746 lines
22 KiB
Rust
746 lines
22 KiB
Rust
// lesavka-uvc - minimal UVC control handler for the gadget node.
|
|
|
|
use anyhow::{Context, Result};
|
|
use std::env;
|
|
use std::fs::OpenOptions;
|
|
use std::os::unix::fs::OpenOptionsExt;
|
|
use std::os::unix::io::AsRawFd;
|
|
use std::thread;
|
|
use std::time::Duration;
|
|
|
|
const STREAM_CTRL_SIZE_11: usize = 26;
|
|
const STREAM_CTRL_SIZE_15: usize = 34;
|
|
const STREAM_CTRL_SIZE_MAX: usize = STREAM_CTRL_SIZE_15;
|
|
const UVC_DATA_SIZE: usize = 60;
|
|
|
|
const V4L2_EVENT_PRIVATE_START: u32 = 0x0800_0000;
|
|
const UVC_EVENT_CONNECT: u32 = V4L2_EVENT_PRIVATE_START + 0;
|
|
const UVC_EVENT_DISCONNECT: u32 = V4L2_EVENT_PRIVATE_START + 1;
|
|
const UVC_EVENT_STREAMON: u32 = V4L2_EVENT_PRIVATE_START + 2;
|
|
const UVC_EVENT_STREAMOFF: u32 = V4L2_EVENT_PRIVATE_START + 3;
|
|
const UVC_EVENT_SETUP: u32 = V4L2_EVENT_PRIVATE_START + 4;
|
|
const UVC_EVENT_DATA: u32 = V4L2_EVENT_PRIVATE_START + 5;
|
|
|
|
const UVC_STRING_CONTROL_IDX: u8 = 0;
|
|
const UVC_STRING_STREAMING_IDX: u8 = 1;
|
|
|
|
const USB_DIR_IN: u8 = 0x80;
|
|
|
|
const UVC_SET_CUR: u8 = 0x01;
|
|
const UVC_GET_CUR: u8 = 0x81;
|
|
const UVC_GET_MIN: u8 = 0x82;
|
|
const UVC_GET_MAX: u8 = 0x83;
|
|
const UVC_GET_RES: u8 = 0x84;
|
|
const UVC_GET_LEN: u8 = 0x85;
|
|
const UVC_GET_INFO: u8 = 0x86;
|
|
const UVC_GET_DEF: u8 = 0x87;
|
|
|
|
const UVC_VS_PROBE_CONTROL: u8 = 0x01;
|
|
const UVC_VS_COMMIT_CONTROL: u8 = 0x02;
|
|
const UVC_VC_REQUEST_ERROR_CODE_CONTROL: u8 = 0x02;
|
|
|
|
#[repr(C)]
|
|
struct V4l2EventSubscription {
|
|
type_: u32,
|
|
id: u32,
|
|
flags: u32,
|
|
reserved: [u32; 5],
|
|
}
|
|
|
|
#[repr(C)]
|
|
union V4l2EventUnion {
|
|
data: [u8; 64],
|
|
_align: u64,
|
|
}
|
|
|
|
#[repr(C)]
|
|
struct V4l2Event {
|
|
type_: u32,
|
|
u: V4l2EventUnion,
|
|
pending: u32,
|
|
sequence: u32,
|
|
timestamp: libc::timespec,
|
|
id: u32,
|
|
reserved: [u32; 8],
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Clone, Copy)]
|
|
struct UsbCtrlRequest {
|
|
b_request_type: u8,
|
|
b_request: u8,
|
|
w_value: u16,
|
|
w_index: u16,
|
|
w_length: u16,
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Clone, Copy)]
|
|
struct UvcRequestData {
|
|
length: i32,
|
|
data: [u8; UVC_DATA_SIZE],
|
|
}
|
|
|
|
#[derive(Clone, Copy)]
|
|
struct UvcConfig {
|
|
width: u32,
|
|
height: u32,
|
|
fps: u32,
|
|
interval: u32,
|
|
max_packet: u32,
|
|
}
|
|
|
|
struct UvcState {
|
|
cfg: UvcConfig,
|
|
ctrl_len: usize,
|
|
default: [u8; STREAM_CTRL_SIZE_MAX],
|
|
probe: [u8; STREAM_CTRL_SIZE_MAX],
|
|
commit: [u8; STREAM_CTRL_SIZE_MAX],
|
|
}
|
|
|
|
#[derive(Clone, Copy)]
|
|
struct PendingRequest {
|
|
interface: u8,
|
|
selector: u8,
|
|
}
|
|
|
|
#[derive(Clone, Copy)]
|
|
struct UvcInterfaces {
|
|
control: u8,
|
|
streaming: u8,
|
|
}
|
|
|
|
fn main() -> Result<()> {
|
|
let (dev, cfg) = parse_args()?;
|
|
let interfaces = load_interfaces();
|
|
eprintln!("[lesavka-uvc] starting (dev={dev})");
|
|
eprintln!(
|
|
"[lesavka-uvc] interfaces control={} streaming={}",
|
|
interfaces.control, interfaces.streaming
|
|
);
|
|
|
|
let debug = env::var("LESAVKA_UVC_DEBUG").is_ok();
|
|
let mut setup_seen: u64 = 0;
|
|
let mut data_seen: u64 = 0;
|
|
let mut dq_err_seen: u64 = 0;
|
|
let mut dq_err_last: Option<i32> = None;
|
|
|
|
loop {
|
|
let file = open_with_retry(&dev)?;
|
|
let fd = file.as_raw_fd();
|
|
let vidioc_subscribe = ioctl_write::<V4l2EventSubscription>(b'V', 90);
|
|
let vidioc_dqevent = ioctl_read::<V4l2Event>(b'V', 89);
|
|
let uvc_send_response = ioctl_write::<UvcRequestData>(b'U', 1);
|
|
|
|
subscribe_event(fd, vidioc_subscribe, UVC_EVENT_SETUP)?;
|
|
subscribe_event(fd, vidioc_subscribe, UVC_EVENT_DATA)?;
|
|
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_CONNECT);
|
|
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_DISCONNECT);
|
|
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_STREAMON);
|
|
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_STREAMOFF);
|
|
|
|
let mut state = UvcState::new(cfg);
|
|
let mut pending: Option<PendingRequest> = None;
|
|
|
|
loop {
|
|
let mut ev = unsafe { std::mem::zeroed::<V4l2Event>() };
|
|
let rc = unsafe { libc::ioctl(fd, vidioc_dqevent, &mut ev) };
|
|
if rc < 0 {
|
|
let err = std::io::Error::last_os_error();
|
|
match err.raw_os_error() {
|
|
Some(libc::EAGAIN) | Some(libc::EINTR) | Some(libc::ENOENT) => {
|
|
if debug {
|
|
let code = err.raw_os_error();
|
|
if dq_err_seen < 10 || code != dq_err_last {
|
|
eprintln!("[lesavka-uvc] dqevent idle: {err}");
|
|
dq_err_seen += 1;
|
|
dq_err_last = code;
|
|
}
|
|
}
|
|
thread::sleep(Duration::from_millis(10));
|
|
continue;
|
|
}
|
|
Some(libc::ENODEV) | Some(libc::EBADF) | Some(libc::EIO) => {
|
|
eprintln!("[lesavka-uvc] device reset ({err}); reopening");
|
|
break;
|
|
}
|
|
_ => {
|
|
eprintln!("[lesavka-uvc] dqevent failed: {err}");
|
|
thread::sleep(Duration::from_millis(100));
|
|
continue;
|
|
}
|
|
}
|
|
}
|
|
|
|
match ev.type_ {
|
|
UVC_EVENT_CONNECT => {
|
|
let speed = u32::from_le_bytes(event_bytes(&ev)[0..4].try_into().unwrap());
|
|
eprintln!("[lesavka-uvc] UVC connect (speed={speed})");
|
|
}
|
|
UVC_EVENT_DISCONNECT => eprintln!("[lesavka-uvc] UVC disconnect"),
|
|
UVC_EVENT_STREAMON => eprintln!("[lesavka-uvc] stream on"),
|
|
UVC_EVENT_STREAMOFF => eprintln!("[lesavka-uvc] stream off"),
|
|
UVC_EVENT_SETUP => {
|
|
let req = parse_ctrl_request(event_bytes(&ev));
|
|
setup_seen += 1;
|
|
if debug || setup_seen <= 10 {
|
|
eprintln!(
|
|
"[lesavka-uvc] setup #{setup_seen} rt=0x{:02x} rq=0x{:02x} val=0x{:04x} idx=0x{:04x} len={}",
|
|
req.b_request_type,
|
|
req.b_request,
|
|
req.w_value,
|
|
req.w_index,
|
|
req.w_length
|
|
);
|
|
}
|
|
handle_setup(
|
|
fd,
|
|
uvc_send_response,
|
|
&mut state,
|
|
&mut pending,
|
|
interfaces,
|
|
req,
|
|
debug,
|
|
);
|
|
}
|
|
UVC_EVENT_DATA => {
|
|
let data = parse_request_data(event_bytes(&ev));
|
|
data_seen += 1;
|
|
if debug || data_seen <= 10 {
|
|
eprintln!(
|
|
"[lesavka-uvc] data #{data_seen} len={}",
|
|
data.length
|
|
);
|
|
}
|
|
handle_data(&mut state, &mut pending, interfaces, data, debug);
|
|
}
|
|
_ => {
|
|
if debug {
|
|
eprintln!("[lesavka-uvc] event type=0x{:08x}", ev.type_);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fn parse_args() -> Result<(String, UvcConfig)> {
|
|
let mut args = env::args().skip(1);
|
|
let mut dev: Option<String> = None;
|
|
|
|
while let Some(arg) = args.next() {
|
|
match arg.as_str() {
|
|
"--device" | "-d" => dev = args.next(),
|
|
_ => dev = Some(arg),
|
|
}
|
|
}
|
|
|
|
let dev = dev
|
|
.or_else(|| env::var("LESAVKA_UVC_DEV").ok())
|
|
.context("missing --device (or LESAVKA_UVC_DEV)")?;
|
|
|
|
Ok((dev, UvcConfig::from_env()))
|
|
}
|
|
|
|
impl UvcConfig {
|
|
fn from_env() -> Self {
|
|
let width = env_u32("LESAVKA_UVC_WIDTH", 1280);
|
|
let height = env_u32("LESAVKA_UVC_HEIGHT", 720);
|
|
let fps = env_u32("LESAVKA_UVC_FPS", 25).max(1);
|
|
let interval = env_u32("LESAVKA_UVC_INTERVAL", 0);
|
|
let mut max_packet = env_u32("LESAVKA_UVC_MAXPACKET", 1024);
|
|
if env::var("LESAVKA_UVC_BULK").is_ok() {
|
|
max_packet = max_packet.min(512);
|
|
} else {
|
|
max_packet = max_packet.min(1024);
|
|
}
|
|
|
|
let interval = if interval == 0 {
|
|
10_000_000 / fps
|
|
} else {
|
|
interval
|
|
};
|
|
|
|
Self {
|
|
width,
|
|
height,
|
|
fps,
|
|
interval,
|
|
max_packet,
|
|
}
|
|
}
|
|
}
|
|
|
|
impl UvcState {
|
|
fn new(cfg: UvcConfig) -> Self {
|
|
let ctrl_len = stream_ctrl_len();
|
|
let default = build_streaming_control(&cfg, ctrl_len);
|
|
Self {
|
|
cfg,
|
|
ctrl_len,
|
|
default,
|
|
probe: default,
|
|
commit: default,
|
|
}
|
|
}
|
|
}
|
|
|
|
fn load_interfaces() -> UvcInterfaces {
|
|
let control = env_u8("LESAVKA_UVC_CTRL_INTF")
|
|
.or_else(|| read_interface("/sys/kernel/config/usb_gadget/lesavka/functions/uvc.usb0/control/bInterfaceNumber"))
|
|
.unwrap_or(UVC_STRING_CONTROL_IDX);
|
|
let streaming = env_u8("LESAVKA_UVC_STREAM_INTF")
|
|
.or_else(|| read_interface("/sys/kernel/config/usb_gadget/lesavka/functions/uvc.usb0/streaming/bInterfaceNumber"))
|
|
.unwrap_or(UVC_STRING_STREAMING_IDX);
|
|
UvcInterfaces { control, streaming }
|
|
}
|
|
|
|
fn read_interface(path: &str) -> Option<u8> {
|
|
std::fs::read_to_string(path)
|
|
.ok()
|
|
.and_then(|v| v.trim().parse::<u8>().ok())
|
|
}
|
|
|
|
fn open_with_retry(path: &str) -> Result<std::fs::File> {
|
|
for attempt in 1..=200 {
|
|
let mut opts = OpenOptions::new();
|
|
opts.read(true).write(true);
|
|
if env::var("LESAVKA_UVC_NONBLOCK").is_ok() {
|
|
opts.custom_flags(libc::O_NONBLOCK);
|
|
}
|
|
match opts.open(path) {
|
|
Ok(f) => {
|
|
eprintln!("[lesavka-uvc] opened {path} (attempt {attempt})");
|
|
return Ok(f);
|
|
}
|
|
Err(err) if err.raw_os_error() == Some(libc::ENOENT) => {
|
|
thread::sleep(Duration::from_millis(50));
|
|
}
|
|
Err(err) => return Err(err).with_context(|| format!("open {path}")),
|
|
}
|
|
}
|
|
Err(anyhow::anyhow!("timeout opening {path}"))
|
|
}
|
|
|
|
fn subscribe_event(fd: i32, req: libc::c_ulong, event: u32) -> Result<()> {
|
|
let mut sub = V4l2EventSubscription {
|
|
type_: event,
|
|
id: 0,
|
|
flags: 0,
|
|
reserved: [0; 5],
|
|
};
|
|
let rc = unsafe { libc::ioctl(fd, req, &mut sub) };
|
|
if rc < 0 {
|
|
return Err(std::io::Error::last_os_error()).with_context(|| {
|
|
format!("subscribe event {event:#x} (fd={fd})")
|
|
});
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn handle_setup(
|
|
fd: i32,
|
|
uvc_send_response: libc::c_ulong,
|
|
state: &mut UvcState,
|
|
pending: &mut Option<PendingRequest>,
|
|
interfaces: UvcInterfaces,
|
|
req: UsbCtrlRequest,
|
|
debug: bool,
|
|
) {
|
|
let interface_lo = (req.w_index & 0xff) as u8;
|
|
let interface_hi = (req.w_index >> 8) as u8;
|
|
let interface_raw = if interface_lo == interfaces.streaming || interface_lo == interfaces.control
|
|
{
|
|
interface_lo
|
|
} else if interface_hi == interfaces.streaming || interface_hi == interfaces.control {
|
|
interface_hi
|
|
} else {
|
|
interface_lo
|
|
};
|
|
let selector = (req.w_value >> 8) as u8;
|
|
let is_in = (req.b_request_type & USB_DIR_IN) != 0;
|
|
if matches!(selector, UVC_VS_PROBE_CONTROL | UVC_VS_COMMIT_CONTROL) {
|
|
maybe_update_ctrl_len(state, req.w_length, debug);
|
|
}
|
|
let interface = map_interface(interface_raw, selector, interfaces, debug);
|
|
|
|
if !is_in && req.b_request == UVC_SET_CUR {
|
|
if interface != interfaces.streaming {
|
|
let _ = send_stall(fd, uvc_send_response);
|
|
return;
|
|
}
|
|
*pending = Some(PendingRequest { interface, selector });
|
|
let len = req.w_length as usize;
|
|
let payload = vec![0u8; len.min(UVC_DATA_SIZE)];
|
|
let _ = send_response(fd, uvc_send_response, &payload);
|
|
if debug {
|
|
eprintln!(
|
|
"[lesavka-uvc] SET_CUR queued len={} iface={} sel={}",
|
|
req.w_length, interface, selector
|
|
);
|
|
}
|
|
return;
|
|
}
|
|
|
|
if !is_in {
|
|
let _ = send_stall(fd, uvc_send_response);
|
|
return;
|
|
}
|
|
|
|
let payload = build_in_response(
|
|
state,
|
|
interfaces,
|
|
interface,
|
|
selector,
|
|
req.b_request,
|
|
req.w_length,
|
|
);
|
|
match payload {
|
|
Some(bytes) => {
|
|
let _ = send_response(fd, uvc_send_response, &bytes);
|
|
}
|
|
None => {
|
|
let _ = send_stall(fd, uvc_send_response);
|
|
}
|
|
}
|
|
}
|
|
|
|
fn map_interface(raw: u8, selector: u8, interfaces: UvcInterfaces, debug: bool) -> u8 {
|
|
if raw == interfaces.streaming || raw == interfaces.control {
|
|
return raw;
|
|
}
|
|
|
|
let mapped = if matches!(selector, UVC_VS_PROBE_CONTROL | UVC_VS_COMMIT_CONTROL) {
|
|
interfaces.streaming
|
|
} else if selector == UVC_VC_REQUEST_ERROR_CODE_CONTROL {
|
|
interfaces.control
|
|
} else {
|
|
raw
|
|
};
|
|
|
|
if debug && mapped != raw {
|
|
eprintln!(
|
|
"[lesavka-uvc] remapped interface {} -> {} for selector {selector}",
|
|
raw, mapped
|
|
);
|
|
}
|
|
|
|
mapped
|
|
}
|
|
|
|
fn maybe_update_ctrl_len(state: &mut UvcState, w_length: u16, debug: bool) {
|
|
let want = w_length as usize;
|
|
if !(want == STREAM_CTRL_SIZE_11 || want == STREAM_CTRL_SIZE_15) {
|
|
return;
|
|
}
|
|
if state.ctrl_len == want {
|
|
return;
|
|
}
|
|
|
|
state.ctrl_len = want;
|
|
state.default = build_streaming_control(&state.cfg, state.ctrl_len);
|
|
state.probe = state.default;
|
|
state.commit = state.default;
|
|
|
|
if debug {
|
|
eprintln!("[lesavka-uvc] ctrl_len set to {}", state.ctrl_len);
|
|
}
|
|
}
|
|
|
|
fn handle_data(
|
|
state: &mut UvcState,
|
|
pending: &mut Option<PendingRequest>,
|
|
interfaces: UvcInterfaces,
|
|
data: UvcRequestData,
|
|
debug: bool,
|
|
) {
|
|
let Some(p) = pending.take() else {
|
|
return;
|
|
};
|
|
|
|
if data.length < 0 {
|
|
return;
|
|
}
|
|
|
|
let len = data.length as usize;
|
|
let slice = &data.data[..len.min(data.data.len())];
|
|
if debug && slice.len() >= STREAM_CTRL_SIZE_11 {
|
|
let interval = read_le32(slice, 4);
|
|
let payload = read_le32(slice, 22);
|
|
eprintln!(
|
|
"[lesavka-uvc] data ctrl fmt={} frame={} interval={} payload={}",
|
|
slice[2],
|
|
slice[3],
|
|
interval,
|
|
payload
|
|
);
|
|
}
|
|
|
|
if p.interface == interfaces.streaming
|
|
&& matches!(p.selector, UVC_VS_PROBE_CONTROL | UVC_VS_COMMIT_CONTROL)
|
|
{
|
|
let sanitized = sanitize_streaming_control(slice, state);
|
|
if p.selector == UVC_VS_PROBE_CONTROL {
|
|
state.probe = sanitized;
|
|
if debug {
|
|
let interval = read_le32(&state.probe, 4);
|
|
let payload = read_le32(&state.probe, 22);
|
|
eprintln!(
|
|
"[lesavka-uvc] probe set interval={} payload={}",
|
|
interval,
|
|
payload
|
|
);
|
|
}
|
|
} else {
|
|
state.commit = sanitized;
|
|
if debug {
|
|
let interval = read_le32(&state.commit, 4);
|
|
let payload = read_le32(&state.commit, 22);
|
|
eprintln!(
|
|
"[lesavka-uvc] commit set interval={} payload={}",
|
|
interval,
|
|
payload
|
|
);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
fn build_in_response(
|
|
state: &UvcState,
|
|
interfaces: UvcInterfaces,
|
|
interface: u8,
|
|
selector: u8,
|
|
request: u8,
|
|
w_length: u16,
|
|
) -> Option<Vec<u8>> {
|
|
let payload = match interface {
|
|
_ if interface == interfaces.streaming => {
|
|
build_streaming_response(state, selector, request)
|
|
}
|
|
_ if interface == interfaces.control => build_control_response(selector, request),
|
|
_ => None,
|
|
}?;
|
|
|
|
Some(adjust_length(payload, w_length))
|
|
}
|
|
|
|
fn build_streaming_response(
|
|
state: &UvcState,
|
|
selector: u8,
|
|
request: u8,
|
|
) -> Option<Vec<u8>> {
|
|
let current = match selector {
|
|
UVC_VS_PROBE_CONTROL => state.probe,
|
|
UVC_VS_COMMIT_CONTROL => state.commit,
|
|
_ => return None,
|
|
};
|
|
|
|
match request {
|
|
UVC_GET_INFO => Some(vec![0x03]),
|
|
UVC_GET_LEN => Some((state.ctrl_len as u16).to_le_bytes().to_vec()),
|
|
UVC_GET_CUR => Some(current[..state.ctrl_len].to_vec()),
|
|
UVC_GET_MIN | UVC_GET_MAX | UVC_GET_DEF | UVC_GET_RES => {
|
|
Some(state.default[..state.ctrl_len].to_vec())
|
|
}
|
|
_ => None,
|
|
}
|
|
}
|
|
|
|
fn build_control_response(selector: u8, request: u8) -> Option<Vec<u8>> {
|
|
match request {
|
|
UVC_GET_INFO => Some(vec![0x01]),
|
|
UVC_GET_LEN => Some(1u16.to_le_bytes().to_vec()),
|
|
UVC_GET_CUR | UVC_GET_MIN | UVC_GET_MAX | UVC_GET_DEF | UVC_GET_RES => {
|
|
if selector == UVC_VC_REQUEST_ERROR_CODE_CONTROL {
|
|
Some(vec![0x00])
|
|
} else {
|
|
Some(vec![0x00])
|
|
}
|
|
}
|
|
_ => None,
|
|
}
|
|
}
|
|
|
|
fn sanitize_streaming_control(
|
|
data: &[u8],
|
|
state: &UvcState,
|
|
) -> [u8; STREAM_CTRL_SIZE_MAX] {
|
|
let mut out = state.default;
|
|
if data.len() >= STREAM_CTRL_SIZE_11 {
|
|
let format_index = data[2];
|
|
let frame_index = data[3];
|
|
let interval = read_le32(data, 4);
|
|
let host_payload = read_le32(data, 22);
|
|
|
|
if format_index == 1 {
|
|
out[2] = 1;
|
|
}
|
|
if frame_index == 1 {
|
|
out[3] = 1;
|
|
}
|
|
if interval != 0 {
|
|
write_le32(&mut out[4..8], interval);
|
|
}
|
|
if host_payload > 0 {
|
|
let payload = host_payload.min(state.cfg.max_packet);
|
|
write_le32(&mut out[22..26], payload);
|
|
}
|
|
}
|
|
out
|
|
}
|
|
|
|
fn send_response(fd: i32, req: libc::c_ulong, payload: &[u8]) -> Result<()> {
|
|
let mut resp = UvcRequestData {
|
|
length: payload.len() as i32,
|
|
data: [0u8; UVC_DATA_SIZE],
|
|
};
|
|
let n = payload.len().min(UVC_DATA_SIZE);
|
|
resp.data[..n].copy_from_slice(&payload[..n]);
|
|
|
|
let rc = unsafe { libc::ioctl(fd, req, &resp) };
|
|
if rc < 0 {
|
|
let err = std::io::Error::last_os_error();
|
|
eprintln!("[lesavka-uvc] send_response failed: {err}");
|
|
return Err(err).context("UVCIOC_SEND_RESPONSE");
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn send_stall(fd: i32, req: libc::c_ulong) -> Result<()> {
|
|
let resp = UvcRequestData {
|
|
length: -1,
|
|
data: [0u8; UVC_DATA_SIZE],
|
|
};
|
|
let rc = unsafe { libc::ioctl(fd, req, &resp) };
|
|
if rc < 0 {
|
|
let err = std::io::Error::last_os_error();
|
|
eprintln!("[lesavka-uvc] send_stall failed: {err}");
|
|
return Err(err).context("UVCIOC_SEND_RESPONSE(stall)");
|
|
}
|
|
Ok(())
|
|
}
|
|
|
|
fn build_streaming_control(cfg: &UvcConfig, ctrl_len: usize) -> [u8; STREAM_CTRL_SIZE_MAX] {
|
|
let mut buf = [0u8; STREAM_CTRL_SIZE_MAX];
|
|
let frame_size = cfg.width * cfg.height * 2;
|
|
|
|
write_le16(&mut buf[0..2], 1); // bmHint: dwFrameInterval
|
|
buf[2] = 1; // bFormatIndex
|
|
buf[3] = 1; // bFrameIndex
|
|
write_le32(&mut buf[4..8], cfg.interval);
|
|
write_le16(&mut buf[8..10], 0);
|
|
write_le16(&mut buf[10..12], 0);
|
|
write_le16(&mut buf[12..14], 0);
|
|
write_le16(&mut buf[14..16], 0);
|
|
write_le16(&mut buf[16..18], 0);
|
|
write_le32(&mut buf[18..22], frame_size);
|
|
write_le32(&mut buf[22..26], cfg.max_packet);
|
|
if ctrl_len >= STREAM_CTRL_SIZE_15 {
|
|
write_le32(&mut buf[26..30], 48_000_000);
|
|
buf[30] = 0;
|
|
buf[31] = 0;
|
|
buf[32] = 0;
|
|
buf[33] = 0;
|
|
}
|
|
|
|
buf
|
|
}
|
|
|
|
fn event_bytes(ev: &V4l2Event) -> [u8; 64] {
|
|
unsafe { ev.u.data }
|
|
}
|
|
|
|
fn parse_ctrl_request(data: [u8; 64]) -> UsbCtrlRequest {
|
|
UsbCtrlRequest {
|
|
b_request_type: data[0],
|
|
b_request: data[1],
|
|
w_value: u16::from_le_bytes([data[2], data[3]]),
|
|
w_index: u16::from_le_bytes([data[4], data[5]]),
|
|
w_length: u16::from_le_bytes([data[6], data[7]]),
|
|
}
|
|
}
|
|
|
|
fn parse_request_data(data: [u8; 64]) -> UvcRequestData {
|
|
let length = i32::from_le_bytes([data[0], data[1], data[2], data[3]]);
|
|
let mut out = [0u8; UVC_DATA_SIZE];
|
|
out.copy_from_slice(&data[4..64]);
|
|
UvcRequestData { length, data: out }
|
|
}
|
|
|
|
fn stream_ctrl_len() -> usize {
|
|
let value = env_u32("LESAVKA_UVC_CTRL_LEN", STREAM_CTRL_SIZE_11 as u32) as usize;
|
|
match value {
|
|
STREAM_CTRL_SIZE_11 | STREAM_CTRL_SIZE_15 => value,
|
|
_ => STREAM_CTRL_SIZE_11,
|
|
}
|
|
}
|
|
|
|
fn env_u32(name: &str, default: u32) -> u32 {
|
|
env::var(name)
|
|
.ok()
|
|
.and_then(|v| v.parse::<u32>().ok())
|
|
.unwrap_or(default)
|
|
}
|
|
|
|
fn env_u8(name: &str) -> Option<u8> {
|
|
env::var(name).ok().and_then(|v| v.parse::<u8>().ok())
|
|
}
|
|
|
|
fn adjust_length(mut bytes: Vec<u8>, w_length: u16) -> Vec<u8> {
|
|
let want = (w_length as usize).min(UVC_DATA_SIZE);
|
|
if bytes.len() > want {
|
|
bytes.truncate(want);
|
|
} else if bytes.len() < want {
|
|
bytes.resize(want, 0);
|
|
}
|
|
bytes
|
|
}
|
|
|
|
fn write_le16(dst: &mut [u8], val: u16) {
|
|
let bytes = val.to_le_bytes();
|
|
dst[0] = bytes[0];
|
|
dst[1] = bytes[1];
|
|
}
|
|
|
|
fn write_le32(dst: &mut [u8], val: u32) {
|
|
let bytes = val.to_le_bytes();
|
|
dst[0] = bytes[0];
|
|
dst[1] = bytes[1];
|
|
dst[2] = bytes[2];
|
|
dst[3] = bytes[3];
|
|
}
|
|
|
|
fn read_le32(src: &[u8], offset: usize) -> u32 {
|
|
u32::from_le_bytes([src[offset], src[offset + 1], src[offset + 2], src[offset + 3]])
|
|
}
|
|
|
|
const IOC_NRBITS: u8 = 8;
|
|
const IOC_TYPEBITS: u8 = 8;
|
|
const IOC_SIZEBITS: u8 = 14;
|
|
const IOC_DIRBITS: u8 = 2;
|
|
|
|
const IOC_NRSHIFT: u8 = 0;
|
|
const IOC_TYPESHIFT: u8 = IOC_NRSHIFT + IOC_NRBITS;
|
|
const IOC_SIZESHIFT: u8 = IOC_TYPESHIFT + IOC_TYPEBITS;
|
|
const IOC_DIRSHIFT: u8 = IOC_SIZESHIFT + IOC_SIZEBITS;
|
|
|
|
const IOC_READ: u8 = 2;
|
|
const IOC_WRITE: u8 = 1;
|
|
|
|
fn ioctl_read<T>(type_: u8, nr: u8) -> libc::c_ulong {
|
|
ioc(IOC_READ, type_, nr, std::mem::size_of::<T>() as u16)
|
|
}
|
|
|
|
fn ioctl_write<T>(type_: u8, nr: u8) -> libc::c_ulong {
|
|
ioc(IOC_WRITE, type_, nr, std::mem::size_of::<T>() as u16)
|
|
}
|
|
|
|
fn ioc(dir: u8, type_: u8, nr: u8, size: u16) -> libc::c_ulong {
|
|
let dir = (dir as u32) << IOC_DIRSHIFT;
|
|
let ty = (type_ as u32) << IOC_TYPESHIFT;
|
|
let nr = (nr as u32) << IOC_NRSHIFT;
|
|
let size = (size as u32) << IOC_SIZESHIFT;
|
|
(dir | ty | nr | size) as libc::c_ulong
|
|
}
|