Poll for new frames with pselect and add timeout

src/lib.rs

@@ -36,11 +36,17 @@ use std::result;
 use std::slice;
 use std::str;
 use std::sync::Arc;
+use std::time::Duration;
+
+use libc::timespec;
+
+use crate::pselect::{make_timespec, pselect, FdSet};
 
 pub use self::consts::*;
 pub use self::v4l2::pubconsts as consts;
 use self::v4l2::MappedRegion;
 
+mod pselect;
 mod v4l2;
 
 pub type Result<T> = result::Result<T, Error>;
@@ -76,6 +82,8 @@ pub struct Config<'a> {
     /// Number of buffers in the queue of camera.
     /// Default is `2`.
     pub nbuffers: u32,
+    /// Timeout for capturing a frame.
+    pub timeout: Option<Duration>,
 }
 
 impl<'a> Default for Config<'a> {
@@ -86,6 +94,7 @@ impl<'a> Default for Config<'a> {
             format: b"YUYV",
             field: FIELD_NONE,
             nbuffers: 2,
+            timeout: None,
         }
     }
 }
@@ -249,19 +258,27 @@ pub struct Camera {
     resolution: (u32, u32),
     format: [u8; 4],
     buffers: Vec<Arc<MappedRegion>>,
+    fd_set: FdSet,
+    timeout: Option<timespec>,
 }
 
 impl Camera {
     pub fn new(device: &str) -> io::Result<Camera> {
+        let fd = v4l2::open(device)?;
+        let mut fd_set = FdSet::new();
+        fd_set.set(fd);
         Ok(Camera {
-            fd: v4l2::open(device)?,
+            fd,
             state: State::Idle,
             resolution: (0, 0),
             format: [0; 4],
             buffers: vec![],
+            fd_set,
+            timeout: None,
         })
     }
 
+    /// Sets the timeout for capturing an image.
     /// Get detailed info about the available formats.
     pub fn formats(&self) -> FormatIter<'_> {
         FormatIter {
@@ -510,6 +527,7 @@ impl Camera {
         self.tune_format(config.resolution, *config.format, config.field)?;
         self.tune_stream(config.interval)?;
         self.alloc_buffers(config.nbuffers)?;
+        self.timeout = config.timeout.map(make_timespec);
 
         if let Err(err) = self.streamon() {
             self.free_buffers();
@@ -534,9 +552,18 @@ impl Camera {
     ///
     /// # Panics
     /// If called w/o streaming.
-    pub fn capture(&self) -> io::Result<Frame> {
+    pub fn capture(&mut self) -> io::Result<Frame> {
         assert_eq!(self.state, State::Streaming);
 
+        pselect(
+            self.fd + 1,
+            Some(&mut self.fd_set),
+            None,
+            None,
+            self.timeout.as_ref(),
+            None,
+        )?;
+
         let mut buf = v4l2::Buffer::new();
 
         v4l2::xioctl(self.fd, v4l2::VIDIOC_DQBUF, &mut buf)?;

src/pselect.rs

@@ -0,0 +1,63 @@
+use std::os::unix::io::RawFd;
+use std::{io, mem, ptr, time};
+
+pub struct FdSet(libc::fd_set);
+
+impl FdSet {
+    pub fn new() -> FdSet {
+        unsafe {
+            let mut raw_fd_set = mem::MaybeUninit::<libc::fd_set>::uninit();
+            libc::FD_ZERO(raw_fd_set.as_mut_ptr());
+            FdSet(raw_fd_set.assume_init())
+        }
+    }
+
+    pub fn set(&mut self, fd: RawFd) {
+        unsafe {
+            libc::FD_SET(fd, &mut self.0);
+        }
+    }
+}
+
+fn to_fdset_ptr(opt: Option<&mut FdSet>) -> *mut libc::fd_set {
+    match opt {
+        None => ptr::null_mut(),
+        Some(&mut FdSet(ref mut raw_fd_set)) => raw_fd_set,
+    }
+}
+fn to_ptr<T>(opt: Option<&T>) -> *const T {
+    match opt {
+        None => ptr::null::<T>(),
+        Some(p) => p,
+    }
+}
+
+pub fn pselect(
+    nfds: libc::c_int,
+    readfds: Option<&mut FdSet>,
+    writefds: Option<&mut FdSet>,
+    errorfds: Option<&mut FdSet>,
+    timeout: Option<&libc::timespec>,
+    sigmask: Option<&libc::sigset_t>,
+) -> io::Result<usize> {
+    match unsafe {
+        libc::pselect(
+            nfds,
+            to_fdset_ptr(readfds),
+            to_fdset_ptr(writefds),
+            to_fdset_ptr(errorfds),
+            to_ptr(timeout),
+            to_ptr(sigmask),
+        )
+    } {
+        -1 => Err(io::Error::last_os_error()),
+        res => Ok(res as usize),
+    }
+}
+
+pub fn make_timespec(duration: time::Duration) -> libc::timespec {
+    libc::timespec {
+        tv_sec: duration.as_secs() as i64,
+        tv_nsec: duration.subsec_nanos() as i64,
+    }
+}

src/v4l2.rs

@@ -6,8 +6,8 @@ use std::ptr::null_mut;
 use std::{io, mem, usize};
 
 // C types and constants.
-use libc::timeval as Timeval;
 use libc::{c_ulong, c_void, off_t, size_t};
+use libc::{timeval as Timeval, O_NONBLOCK};
 use libc::{MAP_SHARED, O_RDWR, PROT_READ, PROT_WRITE};
 
 #[cfg(not(feature = "no_wrapper"))]
@@ -57,7 +57,7 @@ macro_rules! check_io(
 
 pub fn open(file: &str) -> io::Result<RawFd> {
     let c_str = CString::new(file)?;
-    let fd = unsafe { ll::open(c_str.as_ptr(), O_RDWR, 0) };
+    let fd = unsafe { ll::open(c_str.as_ptr(), O_RDWR | O_NONBLOCK, 0) };
     check_io!(fd != -1);
     Ok(fd)
 }