use crate::VideoError;
use std::os::unix::io::AsRawFd;
unsafe extern "C" {
fn ioctl(fd: i32, request: u64, ...) -> i32;
fn mmap(addr: *mut u8, len: usize, prot: i32, flags: i32, fd: i32, offset: i64) -> *mut u8;
fn munmap(addr: *mut u8, len: usize) -> i32;
fn close(fd: i32) -> i32;
}
const VIDIOC_QUERYCAP: u64 = 0x80685600;
const VIDIOC_S_FMT: u64 = 0xC0D05605;
const VIDIOC_REQBUFS: u64 = 0xC0145608;
const VIDIOC_QUERYBUF: u64 = 0xC0585609;
const VIDIOC_QBUF: u64 = 0xC058560F;
const VIDIOC_DQBUF: u64 = 0xC0585611;
const VIDIOC_STREAMON: u64 = 0x40045612;
const VIDIOC_STREAMOFF: u64 = 0x40045613;
const VIDIOC_EXPBUF: u64 = 0xC0405610;
const O_CLOEXEC: u32 = 0o2000000;
const O_RDWR: u32 = 0o2;
const PROT_READ: i32 = 0x1;
const PROT_WRITE: i32 = 0x2;
const MAP_SHARED: i32 = 0x01;
const MAP_FAILED: *mut u8 = !0usize as *mut u8;
const V4L2_BUF_TYPE_VIDEO_CAPTURE: u32 = 1;
const V4L2_MEMORY_MMAP: u32 = 1;
const V4L2_CAP_VIDEO_CAPTURE: u32 = 0x0000_0001;
const V4L2_CAP_STREAMING: u32 = 0x0400_0000;
const NUM_BUFFERS: u32 = 4;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum V4l2PixelFormat {
Yuyv,
Nv12,
Mjpeg,
H264,
}
impl V4l2PixelFormat {
pub const fn fourcc(self) -> u32 {
match self {
Self::Yuyv => 0x5659_5559, Self::Nv12 => 0x3231_564E, Self::Mjpeg => 0x4750_4A4D, Self::H264 => 0x3436_3248, }
}
}
#[repr(C)]
struct V4l2Capability {
driver: [u8; 16],
card: [u8; 32],
bus_info: [u8; 32],
version: u32,
capabilities: u32,
device_caps: u32,
reserved: [u32; 3],
}
#[repr(C)]
#[derive(Clone, Copy)]
struct V4l2PixFormat {
width: u32,
height: u32,
pixelformat: u32,
field: u32,
bytesperline: u32,
sizeimage: u32,
colorspace: u32,
priv_: u32,
flags: u32,
ycbcr_enc: u32,
quantization: u32,
xfer_func: u32,
}
#[repr(C)]
struct V4l2Format {
type_: u32,
pix: V4l2PixFormat,
_pad: [u8; 152],
}
#[repr(C)]
struct V4l2RequestBuffers {
count: u32,
type_: u32,
memory: u32,
capabilities: u32,
flags: u8,
reserved: [u8; 3],
}
#[repr(C)]
#[derive(Clone, Copy, Default)]
struct Timeval {
tv_sec: i64,
tv_usec: i64,
}
#[repr(C)]
struct V4l2Buffer {
index: u32,
type_: u32,
bytesused: u32,
flags: u32,
field: u32,
timestamp: Timeval,
timecode: [u8; 16], sequence: u32,
memory: u32,
m_offset: u64, length: u32,
reserved2: u32,
request_fd: i32,
}
#[repr(C)]
struct V4l2ExportBuffer {
type_: u32,
index: u32,
plane: u32,
flags: u32,
fd: i32,
reserved: [u32; 11],
}
pub struct V4l2Camera {
#[allow(dead_code)]
file: std::fs::File,
fd: i32,
buffers: Vec<(*mut u8, usize)>,
width: u32,
height: u32,
pixel_format: u32,
streaming: bool,
}
unsafe impl Send for V4l2Camera {}
impl V4l2Camera {
pub fn open(
device: &str,
width: u32,
height: u32,
format: V4l2PixelFormat,
) -> Result<Self, VideoError> {
if width == 0 || height == 0 {
return Err(VideoError::InvalidCameraResolution { width, height });
}
let file = std::fs::OpenOptions::new()
.read(true)
.write(true)
.open(device)
.map_err(|e| VideoError::Source(format!("V4L2: cannot open {device}: {e}")))?;
let fd = file.as_raw_fd();
let mut cap: V4l2Capability = unsafe { std::mem::zeroed() };
let ret = unsafe {
ioctl(
fd,
VIDIOC_QUERYCAP,
&mut cap as *mut V4l2Capability as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: QUERYCAP failed".into()));
}
if cap.capabilities & V4L2_CAP_VIDEO_CAPTURE == 0 {
return Err(VideoError::Source(
"V4L2: device does not support video capture".into(),
));
}
if cap.capabilities & V4L2_CAP_STREAMING == 0 {
return Err(VideoError::Source(
"V4L2: device does not support streaming I/O".into(),
));
}
let mut fmt: V4l2Format = unsafe { std::mem::zeroed() };
fmt.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
fmt.pix.width = width;
fmt.pix.height = height;
fmt.pix.pixelformat = format.fourcc();
fmt.pix.field = 1;
let ret = unsafe { ioctl(fd, VIDIOC_S_FMT, &mut fmt as *mut V4l2Format as *mut u8) };
if ret < 0 {
return Err(VideoError::Source("V4L2: S_FMT failed".into()));
}
let actual_w = fmt.pix.width;
let actual_h = fmt.pix.height;
let actual_fmt = fmt.pix.pixelformat;
let mut reqbufs: V4l2RequestBuffers = unsafe { std::mem::zeroed() };
reqbufs.count = NUM_BUFFERS;
reqbufs.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
reqbufs.memory = V4L2_MEMORY_MMAP;
let ret = unsafe {
ioctl(
fd,
VIDIOC_REQBUFS,
&mut reqbufs as *mut V4l2RequestBuffers as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: REQBUFS failed".into()));
}
if reqbufs.count == 0 {
return Err(VideoError::Source(
"V4L2: driver allocated 0 buffers".into(),
));
}
let mut buffers = Vec::with_capacity(reqbufs.count as usize);
for i in 0..reqbufs.count {
let mut buf: V4l2Buffer = unsafe { std::mem::zeroed() };
buf.index = i;
buf.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
let ret = unsafe { ioctl(fd, VIDIOC_QUERYBUF, &mut buf as *mut V4l2Buffer as *mut u8) };
if ret < 0 {
for &(ptr, len) in &buffers {
unsafe {
munmap(ptr, len);
}
}
return Err(VideoError::Source(format!(
"V4L2: QUERYBUF failed for buffer {i}"
)));
}
let ptr = unsafe {
mmap(
std::ptr::null_mut(),
buf.length as usize,
PROT_READ | PROT_WRITE,
MAP_SHARED,
fd,
buf.m_offset as i64,
)
};
if ptr == MAP_FAILED {
for &(p, l) in &buffers {
unsafe {
munmap(p, l);
}
}
return Err(VideoError::Source(format!(
"V4L2: mmap failed for buffer {i}"
)));
}
buffers.push((ptr, buf.length as usize));
}
Ok(Self {
file,
fd,
buffers,
width: actual_w,
height: actual_h,
pixel_format: actual_fmt,
streaming: false,
})
}
pub fn start_streaming(&mut self) -> Result<(), VideoError> {
for i in 0..self.buffers.len() as u32 {
let mut buf: V4l2Buffer = unsafe { std::mem::zeroed() };
buf.index = i;
buf.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
let ret =
unsafe { ioctl(self.fd, VIDIOC_QBUF, &mut buf as *mut V4l2Buffer as *mut u8) };
if ret < 0 {
return Err(VideoError::Source(format!(
"V4L2: QBUF failed for buffer {i}"
)));
}
}
let mut buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
let ret = unsafe {
ioctl(
self.fd,
VIDIOC_STREAMON,
&mut buf_type as *mut u32 as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: STREAMON failed".into()));
}
self.streaming = true;
Ok(())
}
pub fn capture_frame(&mut self) -> Result<&[u8], VideoError> {
if !self.streaming {
return Err(VideoError::Source("V4L2: not streaming".into()));
}
let mut buf: V4l2Buffer = unsafe { std::mem::zeroed() };
buf.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
let ret = unsafe {
ioctl(
self.fd,
VIDIOC_DQBUF,
&mut buf as *mut V4l2Buffer as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: DQBUF failed".into()));
}
let idx = buf.index as usize;
let bytes_used = buf.bytesused as usize;
if idx >= self.buffers.len() {
return Err(VideoError::Source(format!(
"V4L2: DQBUF returned invalid buffer index {idx}"
)));
}
let (ptr, len) = self.buffers[idx];
let actual_len = bytes_used.min(len);
let frame_data = unsafe { std::slice::from_raw_parts(ptr, actual_len) };
let mut qbuf: V4l2Buffer = unsafe { std::mem::zeroed() };
qbuf.index = buf.index;
qbuf.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
qbuf.memory = V4L2_MEMORY_MMAP;
let ret = unsafe {
ioctl(
self.fd,
VIDIOC_QBUF,
&mut qbuf as *mut V4l2Buffer as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: QBUF (re-queue) failed".into()));
}
Ok(frame_data)
}
pub fn capture_frame_indexed(&mut self) -> Result<(u32, &[u8]), VideoError> {
if !self.streaming {
return Err(VideoError::Source("V4L2: not streaming".into()));
}
let mut buf: V4l2Buffer = unsafe { std::mem::zeroed() };
buf.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
buf.memory = V4L2_MEMORY_MMAP;
let ret = unsafe {
ioctl(
self.fd,
VIDIOC_DQBUF,
&mut buf as *mut V4l2Buffer as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: DQBUF failed".into()));
}
let idx = buf.index as usize;
let bytes_used = buf.bytesused as usize;
if idx >= self.buffers.len() {
return Err(VideoError::Source(format!(
"V4L2: DQBUF returned invalid buffer index {idx}"
)));
}
let (ptr, len) = self.buffers[idx];
let actual_len = bytes_used.min(len);
let frame_data = unsafe { std::slice::from_raw_parts(ptr, actual_len) };
let mut qbuf: V4l2Buffer = unsafe { std::mem::zeroed() };
qbuf.index = buf.index;
qbuf.type_ = V4L2_BUF_TYPE_VIDEO_CAPTURE;
qbuf.memory = V4L2_MEMORY_MMAP;
let ret = unsafe {
ioctl(
self.fd,
VIDIOC_QBUF,
&mut qbuf as *mut V4l2Buffer as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: QBUF (re-queue) failed".into()));
}
Ok((buf.index, frame_data))
}
pub fn export_dmabuf(&self, buffer_index: u32) -> Result<i32, VideoError> {
if buffer_index as usize >= self.buffers.len() {
return Err(VideoError::Source(format!(
"V4L2: export_dmabuf buffer index {buffer_index} out of range"
)));
}
let mut ebuf = V4l2ExportBuffer {
type_: V4L2_BUF_TYPE_VIDEO_CAPTURE,
index: buffer_index,
plane: 0,
flags: O_CLOEXEC | O_RDWR,
fd: -1,
reserved: [0; 11],
};
let ret = unsafe {
ioctl(
self.fd,
VIDIOC_EXPBUF,
&mut ebuf as *mut V4l2ExportBuffer as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source(format!(
"V4L2: VIDIOC_EXPBUF failed for buffer {buffer_index}"
)));
}
if ebuf.fd < 0 {
return Err(VideoError::Source(
"V4L2: VIDIOC_EXPBUF returned invalid fd".into(),
));
}
Ok(ebuf.fd)
}
pub fn buffer_mut(&mut self, buffer_index: u32) -> Result<&mut [u8], VideoError> {
let (ptr, len) = self
.buffers
.get(buffer_index as usize)
.copied()
.ok_or_else(|| {
VideoError::Source(format!(
"V4L2: buffer_mut index {buffer_index} out of range"
))
})?;
Ok(unsafe { std::slice::from_raw_parts_mut(ptr, len) })
}
pub fn buffer_count(&self) -> usize {
self.buffers.len()
}
pub fn width(&self) -> u32 {
self.width
}
pub fn height(&self) -> u32 {
self.height
}
pub fn pixel_format_fourcc(&self) -> u32 {
self.pixel_format
}
pub fn stop_streaming(&mut self) -> Result<(), VideoError> {
if !self.streaming {
return Ok(());
}
let mut buf_type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
let ret = unsafe {
ioctl(
self.fd,
VIDIOC_STREAMOFF,
&mut buf_type as *mut u32 as *mut u8,
)
};
if ret < 0 {
return Err(VideoError::Source("V4L2: STREAMOFF failed".into()));
}
self.streaming = false;
Ok(())
}
pub fn export_dmabuf_owned(&self, buffer_index: u32) -> Result<V4l2DmaBufGuard, VideoError> {
let fd = self.export_dmabuf(buffer_index)?;
Ok(V4l2DmaBufGuard { fd })
}
}
pub struct V4l2DmaBufGuard {
fd: i32,
}
impl V4l2DmaBufGuard {
pub fn fd(&self) -> i32 {
self.fd
}
pub fn into_raw(self) -> i32 {
let fd = self.fd;
std::mem::forget(self);
fd
}
}
impl Drop for V4l2DmaBufGuard {
fn drop(&mut self) {
if self.fd >= 0 {
unsafe { close(self.fd) };
}
}
}
unsafe impl Send for V4l2DmaBufGuard {}
unsafe impl Sync for V4l2DmaBufGuard {}
impl Drop for V4l2Camera {
fn drop(&mut self) {
if self.streaming {
let _ = self.stop_streaming();
}
for &(ptr, len) in &self.buffers {
unsafe {
munmap(ptr, len);
}
}
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn v4l2_capability_size() {
assert_eq!(
std::mem::size_of::<V4l2Capability>(),
104,
"V4l2Capability must be 104 bytes to match kernel ABI"
);
}
#[test]
fn v4l2_format_size() {
let size = std::mem::size_of::<V4l2Format>();
assert!(
size >= 204,
"V4l2Format must be at least 204 bytes, got {size}"
);
}
#[test]
fn v4l2_requestbuffers_size() {
assert_eq!(
std::mem::size_of::<V4l2RequestBuffers>(),
20,
"V4l2RequestBuffers must be 20 bytes to match kernel ABI"
);
}
#[test]
fn pixel_format_fourcc_values() {
assert_eq!(V4l2PixelFormat::Yuyv.fourcc(), 0x5659_5559);
assert_eq!(V4l2PixelFormat::Nv12.fourcc(), 0x3231_564E);
assert_eq!(V4l2PixelFormat::Mjpeg.fourcc(), 0x4750_4A4D);
assert_eq!(V4l2PixelFormat::H264.fourcc(), 0x3436_3248);
}
#[test]
fn v4l2_pix_format_size() {
assert_eq!(
std::mem::size_of::<V4l2PixFormat>(),
48,
"V4l2PixFormat must be 48 bytes"
);
}
}