diff options
Diffstat (limited to 'src/v4l2.rs')
| -rw-r--r-- | src/v4l2.rs | 506 |
1 files changed, 506 insertions, 0 deletions
diff --git a/src/v4l2.rs b/src/v4l2.rs new file mode 100644 index 0000000..96cc356 --- /dev/null +++ b/src/v4l2.rs @@ -0,0 +1,506 @@ +use libc as c; +use v4l2_sys as v4l2; +use std::mem::{MaybeUninit, zeroed, replace}; +use std::ptr::null_mut; +use std::slice::from_raw_parts; +use std::os::fd::RawFd; +use std::path::Path; +use std::io::Error as IoError; +use std::fmt::{Display, Debug, Formatter, Error as FmtError}; +use std::error::Error as ErrorTrait; +use std::{str, iter, array}; + +macro_rules! define_flagset{ + ($tname:ident: $ctype:ty; $($name:ident = $mask:expr),+) => { + #[derive(Copy, Clone, Debug, PartialEq, Eq)] + pub struct $tname{ $($name: bool),+ } + impl $tname{ + pub fn zero() -> Self{ + Self{ $($name: false),+ } + } + } + impl From<$ctype> for $tname{ + fn from(src: $ctype) -> Self{ + let mut ret = Self::zero(); + $( + if src & $mask != 0{ + ret.$name = true; + } + )+ + ret + } + } + impl From<$tname> for $ctype{ + fn from(src: $tname) -> Self{ + let mut ret = 0; + $( + if src.$name{ + ret |= $mask; + } + )+ + ret + } + } + }; +} +macro_rules! define_cenum{ + ($tname:ident: $ctype:ty; $($name:ident = $mask:expr),+) => { + #[derive(Copy, Clone, Debug, PartialEq, Eq)] + #[repr($ctype)] + pub enum $tname{ + $($name = $mask),+ + } + impl From<$ctype> for $tname{ + fn from(src: $ctype) -> Self{ + $(if src == $mask{ + return Self::$name; + })+ + panic!("{} is invalid value for {}", src, stringify!($tname)); + } + } + impl From<$tname> for $ctype{ + fn from(src: $tname) -> Self{ + src as Self + } + } + }; +} + +define_flagset!{ BufFlags: u32; + mapped = v4l2::V4L2_BUF_FLAG_MAPPED, + queued = v4l2::V4L2_BUF_FLAG_QUEUED, + done = v4l2::V4L2_BUF_FLAG_DONE, + error = v4l2::V4L2_BUF_FLAG_ERROR, + keyframe = v4l2::V4L2_BUF_FLAG_KEYFRAME, + pframe = v4l2::V4L2_BUF_FLAG_PFRAME, + bframe = v4l2::V4L2_BUF_FLAG_BFRAME, + timecode = v4l2::V4L2_BUF_FLAG_TIMECODE, + prepared = v4l2::V4L2_BUF_FLAG_PREPARED, + no_cache_invalidate = v4l2::V4L2_BUF_FLAG_NO_CACHE_INVALIDATE, + no_cache_clean = v4l2::V4L2_BUF_FLAG_NO_CACHE_CLEAN, + last = v4l2::V4L2_BUF_FLAG_LAST +} +define_cenum!{ Field: u32; + Any = v4l2::v4l2_field_V4L2_FIELD_ANY, + None = v4l2::v4l2_field_V4L2_FIELD_NONE, + Top = v4l2::v4l2_field_V4L2_FIELD_TOP, + Bottom = v4l2::v4l2_field_V4L2_FIELD_BOTTOM, + Interlaced = v4l2::v4l2_field_V4L2_FIELD_INTERLACED, + SeqTb = v4l2::v4l2_field_V4L2_FIELD_SEQ_TB, + SeqBt = v4l2::v4l2_field_V4L2_FIELD_SEQ_BT, + Alternate = v4l2::v4l2_field_V4L2_FIELD_ALTERNATE, + InterlacedTb = v4l2::v4l2_field_V4L2_FIELD_INTERLACED_TB, + InterlacedBt = v4l2::v4l2_field_V4L2_FIELD_INTERLACED_BT +} +#[derive(Copy, Clone, PartialEq, Eq)] +pub struct ImageFormat([u8; 4]); +impl ImageFormat{ + pub fn slice(&self) -> &[u8]{ + let p = self.0.iter().rev() + .position(|c| *c != 0x20) + .unwrap_or(4); + &self.0[..4-p] + } + pub fn as_str(&self) -> &str{ + // XXX: it is invalid when self.be() + str::from_utf8(self.slice()).unwrap() + } + pub fn name(&self) -> String{ + let s = self.slice(); + let v = iter::once(s[0] & 0x7f).chain(s[1..].iter().copied()).collect(); + String::from_utf8(v).unwrap() + } + pub fn be(&self) -> bool{ + self.0[0] & 0x80 != 0 + } +} +impl From<&str> for ImageFormat{ + fn from(mut src: &str) -> Self{ + let be = + if src.ends_with(":be"){ + let l = src.len(); + src = &src[..l-3]; + true + }else{ + false + }; + assert!(src.chars().skip(4).next().is_none()); + let mut a: [u8; 4] = array::from_fn( + |n| src.chars().skip(n).next().unwrap_or(' ') as u8); + if be{ a[0] |= 0x80; } + Self(a) + } +} +impl From<u32> for ImageFormat{ + fn from(src: u32) -> Self{ + Self(src.to_le_bytes()) + } +} +impl From<ImageFormat> for u32{ + fn from(src: ImageFormat) -> Self{ + Self::from_le_bytes(src.0) + } +} +impl Debug for ImageFormat{ + fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), FmtError>{ + write!(f, "ImageFormat({}{})", + self.name(), + if self.be(){ ":be" }else{ "" }) + } +} + +#[derive(Copy, Clone)] +pub struct BufAttrs{ + pub flags: BufFlags, + pub field: Field, + pub timestamp: c::timeval, + pub sequence: u32, +} +impl Debug for BufAttrs{ + fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), FmtError>{ + #[derive(Debug)] + #[allow(non_camel_case_types,unused)] + struct timeval{ + tv_sec: c::time_t, + tv_usec: c::suseconds_t, + } + f.debug_struct("BufAttrs") + .field("flags", &self.flags) + .field("field", &self.field) + .field("timestamp", &timeval{ + tv_sec: self.timestamp.tv_sec, + tv_usec: self.timestamp.tv_usec, + }) + .field("sequence", &self.sequence) + .finish() + } +} + +#[derive(Copy, Clone, Debug)] +struct MmappedBuffer{ + ptr: *mut u8, + length: u32, +} + +#[derive(Clone, Debug)] +enum IoMethod{ + Uninit, + Mmap(/* unsafe to construct */ Vec<MmappedBuffer>), +} + +#[derive(Debug)] +pub struct Device{ + fd: RawFd, + cap: v4l2::v4l2_capability, + io_capture: IoMethod, +} + +macro_rules! /* unsafe */ mk_ioctl_getter{ + ($name:ident, $type:ty, $op:expr; $($qn:ident: $qt:ty),*) => { + fn $name(&self, $($qn: $qt),*) -> Result<$type, IoError>{ + let mut val = MaybeUninit::<$type>::uninit(); + $(unsafe{ (&raw mut (*val.as_mut_ptr()).$qn).write($qn) };)* + + if unsafe{ c::ioctl(self.fd, $op, val.as_mut_ptr()) } < 0{ + Err(IoError::last_os_error()) + }else{ + Ok(unsafe{ val.assume_init() }) + } + } + }; +} +macro_rules! /* unsafe */ mk_ioctl_setter{ + ($name:ident, $type:ty, $op:expr) => { + fn $name(&self, mut val: $type) -> Result<$type, IoError>{ + if unsafe{ c::ioctl(self.fd, $op, &mut val as *mut $type) } < 0{ + Err(IoError::last_os_error()) + }else{ + Ok(val) + } + } + }; +} + +impl Device{ + pub unsafe fn from_rawfd(fd: RawFd) -> Result<Device, IoError>{ + let mut cap = MaybeUninit::<v4l2::v4l2_capability>::uninit(); + if unsafe{c::ioctl(fd, v4l2::VIDIOC_QUERYCAP, cap.as_mut_ptr())} < 0{ + return Err(IoError::last_os_error()); + } + let cap = cap.assume_init(); + + Ok(Device{ fd, cap, + io_capture: IoMethod::Uninit, + }) + } + pub fn open(path: impl AsRef<Path>) -> Result<Device, IoError>{ + let pathstr_bytes = path.as_ref().as_os_str().as_encoded_bytes(); + let p_bytes: Vec<u8> = pathstr_bytes.iter() + .copied() + .chain(iter::once(0)) + .collect(); + + let fd = unsafe{c::open( + p_bytes.as_ptr() as *const c::c_char, + c::O_RDWR)}; + if fd < 0{ + return Err(IoError::last_os_error()); + } + + unsafe{Device::from_rawfd(fd)} + } + mk_ioctl_getter!(ioctl_get_format, v4l2::v4l2_format, v4l2::VIDIOC_G_FMT; + type_: u32); + mk_ioctl_setter!(ioctl_set_format, v4l2::v4l2_format, v4l2::VIDIOC_S_FMT); + //mk_ioctl_getter!(ioctl_get_streamparm, + // v4l2::v4l2_streamparm, v4l2::VIDIOC_G_PARM; + // type_: u32); + //mk_ioctl_setter!(ioctl_set_streamparm, + // v4l2::v4l2_streamparm, v4l2::VIDIOC_S_PARM); + mk_ioctl_setter!(ioctl_req_buffers, + v4l2::v4l2_requestbuffers, v4l2::VIDIOC_REQBUFS); + mk_ioctl_setter!(ioctl_query_buffer, + v4l2::v4l2_buffer, v4l2::VIDIOC_QUERYBUF); + mk_ioctl_setter!(ioctl_queue_buffer, + v4l2::v4l2_buffer, v4l2::VIDIOC_QBUF); + mk_ioctl_getter!(ioctl_dequeue_buffer, + v4l2::v4l2_buffer, v4l2::VIDIOC_DQBUF; + type_: u32); + + unsafe fn unmap_bufs(bufs: Vec<MmappedBuffer>){ + for buf in bufs{ + if unsafe{ c::munmap( + buf.ptr as *mut c::c_void, buf.length as usize) } < 0{ + panic!("munmap: {:?}", IoError::last_os_error()); + } + } + } + fn uninit_io(&self, io: IoMethod, ty: u32) -> Result<(), IoError>{ + match io{ + IoMethod::Uninit => (), + IoMethod::Mmap(bufs) => { + unsafe{ Self::unmap_bufs(bufs) }; + if unsafe{ c::ioctl(self.fd, v4l2::VIDIOC_STREAMOFF, + &raw const ty as *const c::c_int) } < 0{ + return Err(IoError::last_os_error()); + } + }, + }; + Ok(()) + } + fn init_mmap_input(&self, ty: u32) -> Result<IoMethod, IoError>{ + let mut req = unsafe{ zeroed::<v4l2::v4l2_requestbuffers>() }; + req.count = 4; + req.type_ = ty; + req.memory = v4l2::v4l2_memory_V4L2_MEMORY_MMAP; + + let count = self.ioctl_req_buffers(req)?.count; + if count == 0{ + return Ok(IoMethod::Uninit); + } + + let mut bufrs = Vec::with_capacity(count as usize); + for i in 0..count{ + let mut buf = unsafe{ zeroed::<v4l2::v4l2_buffer>() }; + buf.type_ = ty; + buf.memory = v4l2::v4l2_memory_V4L2_MEMORY_MMAP; + buf.index = i; + let buf = self.ioctl_query_buffer(buf)?; + bufrs.push(buf); + + self.ioctl_queue_buffer(buf)?; + } + + if unsafe{ c::ioctl(self.fd, v4l2::VIDIOC_STREAMON, + &raw const ty as *const c::c_int) } < 0{ + return Err(IoError::last_os_error()); + } + + let mut bufs = Vec::with_capacity(count as usize); + for bufr in bufrs{ + let ptr = unsafe{ c::mmap(null_mut(), bufr.length as c::size_t, + c::PROT_READ | c::PROT_WRITE, c::MAP_SHARED, + self.fd, bufr.m.offset as c::off_t) }; + if ptr == c::MAP_FAILED{ + unsafe{ Self::unmap_bufs(bufs) }; + panic!("mmap: {:?}", IoError::last_os_error()); + } + bufs.push(MmappedBuffer{ptr: ptr as *mut u8, length: bufr.length}); + } + + Ok(IoMethod::Mmap(bufs)) + } + + fn dequeue<R>(&self, io: &IoMethod, ty: u32, + cb: impl FnOnce(&[u8], BufAttrs)->R + ) -> Result<R, IoError>{ + let bufr = self.ioctl_dequeue_buffer(ty)?; + let attrs = BufAttrs{ + flags: bufr.flags.into(), + field: bufr.field.into(), + timestamp: c::timeval{ + tv_sec: bufr.timestamp.tv_sec, + tv_usec: bufr.timestamp.tv_usec, + }, + sequence: bufr.sequence, + }; + let ret; + match io{ + IoMethod::Uninit => panic!(), + IoMethod::Mmap(ref mmb) => { + let buf = mmb[bufr.index as usize]; + assert!(bufr.bytesused <= buf.length); + let slice = unsafe{ from_raw_parts( + buf.ptr as *const u8, + bufr.bytesused as usize) }; + ret = cb(slice, attrs); + }, + } + self.ioctl_queue_buffer(bufr)?; + Ok(ret) + } + + pub fn captstream_builder(self) + -> Result<CaptStreamBuilder, CaptStreamBuilderNewError>{ + CaptStreamBuilder::new(self) + } +} +impl Drop for Device{ + fn drop(&mut self){ + let io_capture = replace(&mut self.io_capture, IoMethod::Uninit); + let _ = self.uninit_io(io_capture, + v4l2::v4l2_buf_type_V4L2_BUF_TYPE_VIDEO_CAPTURE); + // skip close error check + unsafe{ c::close(self.fd) }; + } +} + +#[derive(Debug)] +pub enum CaptStreamBuilderNewError{ + NoVideoCaptureCapability, + NoStreamingCapability, + IoError(IoError), +} +impl Display for CaptStreamBuilderNewError{ + fn fmt(&self, f: &mut Formatter<'_>) -> Result<(), FmtError>{ + Debug::fmt(self, f) // TODO + } +} +impl ErrorTrait for CaptStreamBuilderNewError{} +impl From<IoError> for CaptStreamBuilderNewError{ + fn from(src: IoError) -> Self{ + Self::IoError(src) + } +} +pub struct CaptStreamBuilder{ + v4l2: Device, + pix_format: v4l2::v4l2_pix_format, + dirty: bool, +} +impl CaptStreamBuilder{ + pub fn new(v4l2: Device) -> Result<Self, CaptStreamBuilderNewError>{ + if v4l2.cap.capabilities & v4l2::V4L2_CAP_VIDEO_CAPTURE == 0{ + return Err(CaptStreamBuilderNewError::NoVideoCaptureCapability); + } + if v4l2.cap.capabilities & v4l2::V4L2_CAP_STREAMING == 0{ + return Err(CaptStreamBuilderNewError::NoStreamingCapability); + } + let fmt = v4l2.ioctl_get_format( + v4l2::v4l2_buf_type_V4L2_BUF_TYPE_VIDEO_CAPTURE)?; + assert!(fmt.type_ == v4l2::v4l2_buf_type_V4L2_BUF_TYPE_VIDEO_CAPTURE); + Ok(Self{ + v4l2, + pix_format: unsafe{ fmt.fmt.pix }, + dirty: false, + }) + } + pub fn build(self) -> Result<CaptStream, IoError>{ + let mut v4l2 = self.v4l2; + let mut pix_format = self.pix_format; + if self.dirty{ + let mut fmt: v4l2::v4l2_format = unsafe{ zeroed() }; + fmt.type_ = v4l2::v4l2_buf_type_V4L2_BUF_TYPE_VIDEO_CAPTURE; + fmt.fmt.pix = pix_format; + let fmt = v4l2.ioctl_set_format(fmt)?; + assert!(fmt.type_ + == v4l2::v4l2_buf_type_V4L2_BUF_TYPE_VIDEO_CAPTURE); + pix_format = unsafe{ fmt.fmt.pix }; + } + v4l2.io_capture = v4l2.init_mmap_input( + v4l2::v4l2_buf_type_V4L2_BUF_TYPE_VIDEO_CAPTURE)?; + Ok(CaptStream{ v4l2, pix_format }) + } + + pub fn set_width(mut self, v: usize) -> Self{ + self.pix_format.width = v as u32; + self.dirty = true; + self + } + pub fn set_height(mut self, v: usize) -> Self{ + self.pix_format.height = v as u32; + self.dirty = true; + self + } + pub fn set_pixelformat(mut self, v: ImageFormat) -> Self{ + self.pix_format.pixelformat = v.into(); + self.pix_format.bytesperline = 0; + self.dirty = true; + self + } + pub fn set_field(mut self, v: Field) -> Self{ + self.pix_format.field = v.into(); + self.dirty = true; + self + } + pub fn set_bytesperline(mut self, v: usize) -> Self{ + self.pix_format.bytesperline = v as u32; + self.dirty = true; + self + } + pub fn set_sizeimage(mut self, v: usize) -> Self{ + self.pix_format.sizeimage = v as u32; + self.dirty = true; + self + } +} + +macro_rules! impl_pix_format_reader{ + (+++ $n:ident: usize) => { + pub fn $n(&self) -> usize{ + self.format().$n as usize + } + }; + (+++ $n:ident: $t:ty) => { + pub fn $n(&self) -> $t{ + self.format().$n.into() + } + }; + ($t:ty) => { + impl $t{ + pub fn format(&self) -> v4l2::v4l2_pix_format{ + self.pix_format + } + impl_pix_format_reader!(+++ width: usize); + impl_pix_format_reader!(+++ height: usize); + impl_pix_format_reader!(+++ pixelformat: ImageFormat); + impl_pix_format_reader!(+++ field: Field); + impl_pix_format_reader!(+++ bytesperline: usize); + impl_pix_format_reader!(+++ sizeimage: usize); + //impl_pix_format_reader!(+++ colorspace: ColorSpace); + } + }; +} +impl_pix_format_reader!(CaptStreamBuilder); + +pub struct CaptStream{ + v4l2: Device, + pix_format: v4l2::v4l2_pix_format, +} +impl CaptStream{ + pub fn next<R>(&self, cb: impl FnOnce(&[u8], BufAttrs)->R) + -> Result<R, IoError>{ + self.v4l2.dequeue(&self.v4l2.io_capture, + v4l2::v4l2_buf_type_V4L2_BUF_TYPE_VIDEO_CAPTURE, cb) + } +} +impl_pix_format_reader!(CaptStream); |