vhost_user_media: Add lens facing control, worker POLLHUP handling, and shmem_unmap fix

base/cvd/cuttlefish/host/commands/vhost_user_media/v4l2_stream_proxy/src/device.rs

@@ -12,15 +12,14 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+use nix::sys::eventfd::{EfdFlags, EventFd};
 use std::collections::VecDeque;
 use std::io::Result as IoResult;
 use std::os::fd::AsFd;
 use std::os::fd::BorrowedFd;
-use std::sync::{Arc, Mutex};
 use std::sync::mpsc::channel;
+use std::sync::{Arc, Mutex};
 use std::time::Instant;
-use nix::sys::eventfd::{EventFd, EfdFlags};
-
 
 use v4l2r::PixelFormat;
 use v4l2r::QueueType;
@@ -31,6 +30,9 @@ use v4l2r::bindings::v4l2_requestbuffers;
 use v4l2r::ioctl::BufferCapabilities;
 use v4l2r::ioctl::BufferField;
 use v4l2r::ioctl::BufferFlags;
+use v4l2r::ioctl::CtrlId;
+use v4l2r::ioctl::CtrlWhich;
+use v4l2r::ioctl::QueryCtrlFlags;
 use v4l2r::ioctl::V4l2Buffer;
 use v4l2r::ioctl::V4l2PlanesWithBackingMut;
 use v4l2r::memory::MemoryType;
@@ -94,11 +96,7 @@ impl Format {
         let w = width as usize;
         let h = height as usize;
         match self {
-            Self::Yuv420M => vec![
-                w * h,
-                w * h / 4,
-                w * h / 4,
-            ],
+            Self::Yuv420M => vec![w * h, w * h / 4, w * h / 4],
         }
     }
 
@@ -179,9 +177,11 @@ impl Buffer {
                     tv_usec: (ts.tv_nsec / 1000) as bindings::__time_t,
                 });
                 Self::unset_flag(&mut flags, BufferFlags::QUEUED);
-                
+
                 // Set bytesused to plane length for all planes
-                if let V4l2PlanesWithBackingMut::Mmap(planes) = self.v4l2_buffer.planes_with_backing_iter_mut() {
+                if let V4l2PlanesWithBackingMut::Mmap(planes) =
+                    self.v4l2_buffer.planes_with_backing_iter_mut()
+                {
                     for mut plane in planes {
                         let len = *plane.length;
                         *plane.bytesused = len;
@@ -194,7 +194,9 @@ impl Buffer {
     }
 
     fn clear_bytesused(&mut self) {
-        if let V4l2PlanesWithBackingMut::Mmap(planes) = self.v4l2_buffer.planes_with_backing_iter_mut() {
+        if let V4l2PlanesWithBackingMut::Mmap(planes) =
+            self.v4l2_buffer.planes_with_backing_iter_mut()
+        {
             for mut plane in planes {
                 *plane.bytesused = 0;
             }
@@ -270,13 +272,16 @@ where
     }
 
     fn default_fmt(&self, queue: QueueType) -> v4l2_format {
-        let plane_sizes = self.config.format.plane_sizes(self.config.input_width, self.config.input_height);
+        let plane_sizes = self
+            .config
+            .format
+            .plane_sizes(self.config.input_width, self.config.input_height);
         let mut plane_fmt: [bindings::v4l2_plane_pix_format; 8] = Default::default();
         for (i, &size) in plane_sizes.iter().enumerate() {
             plane_fmt[i].sizeimage = size as u32;
             plane_fmt[i].bytesperline = self.config.format.bytesperline(self.config.input_width, i);
         }
-        
+
         let pix_mp = bindings::v4l2_pix_format_mplane {
             width: self.config.input_width,
             height: self.config.input_height,
@@ -421,11 +426,7 @@ where
         Ok(self.default_fmtdesc(queue))
     }
 
-    fn g_fmt(
-        &mut self,
-        _session: &Self::Session,
-        queue: QueueType,
-    ) -> IoctlResult<v4l2_format> {
+    fn g_fmt(&mut self, _session: &Self::Session, queue: QueueType) -> IoctlResult<v4l2_format> {
         if queue != self.queue_type() {
             return Err(libc::EINVAL);
         }
@@ -549,19 +550,21 @@ where
             }
         }
 
-        let plane_sizes = self.config.format.plane_sizes(self.config.input_width, self.config.input_height);
+        let plane_sizes = self
+            .config
+            .format
+            .plane_sizes(self.config.input_width, self.config.input_height);
         let num_planes = plane_sizes.len();
 
         state.buffers = (0..count)
             .map(|i| -> Result<Buffer, i32> {
                 let mut planes = Vec::new();
-                
+
                 for &size in &plane_sizes {
-                    let fd = MemFdBuffer::new(size as u64)
-                        .map_err(|e| {
-                            log::error!("failed to allocate MMAP buffer: {:#}", e);
-                            libc::ENOMEM
-                        })?;
+                    let fd = MemFdBuffer::new(size as u64).map_err(|e| {
+                        log::error!("failed to allocate MMAP buffer: {:#}", e);
+                        libc::ENOMEM
+                    })?;
                     let offset = self
                         .mmap_manager
                         .register_buffer(None, size as u32)
@@ -570,7 +573,7 @@ where
                 }
 
                 let mut v4l2_buffer = V4l2Buffer::new(expected_queue, i, MemoryType::Mmap);
-                
+
                 if num_planes > 1 {
                     unsafe {
                         (*v4l2_buffer.as_mut_ptr()).length = num_planes as u32;
@@ -596,7 +599,7 @@ where
                         panic!()
                     }
                 }
-                
+
                 v4l2_buffer.set_field(BufferField::None);
                 v4l2_buffer.set_flags(BufferFlags::TIMESTAMP_MONOTONIC);
 
@@ -638,7 +641,7 @@ where
     ) -> IoctlResult<V4l2Buffer> {
         let mut state = session.state.lock().unwrap();
         let buf_id = qbuf.index() as usize;
-        
+
         let buf_v4l2 = {
             let buffer = state.buffers.get_mut(buf_id).ok_or(libc::EINVAL)?;
             if buffer.state == BufferState::Incoming {
@@ -647,9 +650,9 @@ where
             buffer.set_state(BufferState::Incoming);
             buffer.v4l2_buffer.clone()
         };
-        
+
         state.queued_buffers.push_back(buf_id);
-        
+
         if session.streaming {
             if let Some(ref worker) = session.worker {
                 let _ = worker.tx.send(WorkerCmd::BufferQueued);
@@ -691,10 +694,20 @@ where
         let config_clone = self.config.clone();
 
         let join_handle = std::thread::spawn(move || {
-            worker_thread_loop(config_clone, evt_queue_clone, state_clone, rx, event_fd_clone);
+            worker_thread_loop(
+                config_clone,
+                evt_queue_clone,
+                state_clone,
+                rx,
+                event_fd_clone,
+            );
         });
 
-        session.worker = Some(WorkerHandle { tx, event_fd, join_handle });
+        session.worker = Some(WorkerHandle {
+            tx,
+            event_fd,
+            join_handle,
+        });
 
         Ok(())
     }
@@ -799,4 +812,135 @@ where
             ..Default::default()
         })
     }
+
+    fn query_ext_ctrl(
+        &mut self,
+        _session: &Self::Session,
+        id: CtrlId,
+        flags: QueryCtrlFlags,
+    ) -> IoctlResult<bindings::v4l2_query_ext_ctrl> {
+        let requested_id: u32 = unsafe { std::mem::transmute(id) };
+
+        if flags.contains(QueryCtrlFlags::NEXT) {
+            if requested_id < CID_LENS_FACING {
+                return Ok(self.lens_facing_query_ext_ctrl());
+            }
+        } else if requested_id == CID_LENS_FACING {
+            return Ok(self.lens_facing_query_ext_ctrl());
+        }
+
+        Err(libc::EINVAL)
+    }
+
+    fn g_ctrl(&mut self, _session: &Self::Session, id: u32) -> IoctlResult<bindings::v4l2_control> {
+        if id == CID_LENS_FACING {
+            return Ok(bindings::v4l2_control {
+                id,
+                value: self.config.lens_facing as i32,
+            });
+        }
+        Err(libc::EINVAL)
+    }
+
+    fn g_ext_ctrls(
+        &mut self,
+        _session: &Self::Session,
+        _which: CtrlWhich,
+        _ctrls: &mut bindings::v4l2_ext_controls,
+        ctrl_array: &mut Vec<bindings::v4l2_ext_control>,
+        _user_regions: Vec<Vec<SgEntry>>,
+    ) -> IoctlResult<()> {
+        for ctrl in ctrl_array.iter_mut() {
+            if ctrl.id == CID_LENS_FACING {
+                ctrl.__bindgen_anon_1.value64 = self.config.lens_facing as i64;
+            } else {
+                return Err(libc::EINVAL);
+            }
+        }
+        Ok(())
+    }
+}
+
+/// https://developer.android.com/reference/android/hardware/camera2/CameraMetadata#LENS_FACING_FRONT
+#[derive(Debug, Clone, Copy, PartialEq, Eq)]
+pub enum LensFacing {
+    Front = 0,
+    Back = 1,
+    External = 2,
+}
+
+impl std::str::FromStr for LensFacing {
+    type Err = String;
+
+    fn from_str(s: &str) -> Result<Self, Self::Err> {
+        match s {
+            "FRONT" => Ok(LensFacing::Front),
+            "BACK" => Ok(LensFacing::Back),
+            "EXTERNAL" => Ok(LensFacing::External),
+            _ => Err(format!(
+                "Invalid lens facing: {}. Expected FRONT, BACK, or EXTERNAL",
+                s
+            )),
+        }
+    }
+}
+
+const CID_OFFSET: u32 = bindings::V4L2_CID_CAMERA_CLASS_BASE + 0x100;
+const CID_LENS_FACING: u32 = CID_OFFSET + 1;
+
+fn ctrl_name(name: &str) -> [i8; 32] {
+    let mut array = [0i8; 32];
+    let bytes = name.as_bytes();
+    let len = std::cmp::min(bytes.len(), 31);
+    for i in 0..len {
+        array[i] = bytes[i] as i8;
+    }
+    array
+}
+
+impl<Q: VirtioMediaEventQueue, HM: VirtioMediaHostMemoryMapper> V4l2Stream<Q, HM> {
+    fn lens_facing_query_ext_ctrl(&self) -> bindings::v4l2_query_ext_ctrl {
+        bindings::v4l2_query_ext_ctrl {
+            id: CID_LENS_FACING,
+            type_: bindings::v4l2_ctrl_type_V4L2_CTRL_TYPE_INTEGER,
+            name: ctrl_name("LENS_FACING"),
+            minimum: 0,
+            maximum: 2,
+            step: 1,
+            default_value: self.config.lens_facing as i64,
+            flags: bindings::V4L2_CTRL_FLAG_READ_ONLY,
+            elems: 1,
+            elem_size: std::mem::size_of::<u32>() as u32,
+            ..Default::default()
+        }
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::*;
+
+    #[test]
+    fn test_lens_facing_from_str() {
+        assert_eq!("FRONT".parse::<LensFacing>().unwrap(), LensFacing::Front);
+        assert_eq!("BACK".parse::<LensFacing>().unwrap(), LensFacing::Back);
+        assert_eq!(
+            "EXTERNAL".parse::<LensFacing>().unwrap(),
+            LensFacing::External
+        );
+        assert!("INVALID".parse::<LensFacing>().is_err());
+    }
+
+    #[test]
+    fn test_ctrl_name_is_nul_padded() {
+        let name = ctrl_name("LENS_FACING");
+        assert_eq!(&name[..11], b"LENS_FACING".map(|b| b as i8));
+        assert!(name[11..].iter().all(|byte| *byte == 0));
+    }
+
+    #[test]
+    fn test_ctrl_name_truncates_and_stays_nul_terminated() {
+        let name = ctrl_name("This control name is definitely far too long to fit");
+        assert_eq!(name[31], 0);
+    }
 }