1071 lines
33 KiB
C++
1071 lines
33 KiB
C++
// 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;
|
||
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 CONFIGFS_UVC_BASE: &str = "/sys/kernel/config/usb_gadget/lesavka/functions/uvc.usb0";
|
||
|
||
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,
|
||
frame_size: u32,
|
||
}
|
||
|
||
struct PayloadCap {
|
||
limit: u32,
|
||
pct: u32,
|
||
source: &'static str,
|
||
periodic_dw: Option<u32>,
|
||
non_periodic_dw: Option<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],
|
||
cfg_snapshot: Option<ConfigfsSnapshot>,
|
||
}
|
||
|
||
#[derive(Clone, Copy)]
|
||
struct PendingRequest {
|
||
interface: u8,
|
||
selector: u8,
|
||
expected_len: usize,
|
||
}
|
||
|
||
#[derive(Clone, Copy)]
|
||
struct UvcInterfaces {
|
||
control: u8,
|
||
streaming: u8,
|
||
}
|
||
|
||
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
|
||
struct ConfigfsSnapshot {
|
||
width: u32,
|
||
height: u32,
|
||
default_interval: u32,
|
||
frame_interval: u32,
|
||
maxpacket: u32,
|
||
maxburst: u32,
|
||
}
|
||
|
||
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 nonblock = env::var("LESAVKA_UVC_BLOCKING").is_err();
|
||
eprintln!("[lesavka-uvc] nonblock={}", if nonblock { 1 } else { 0 });
|
||
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 = match open_with_retry(&dev) {
|
||
Ok(file) => file,
|
||
Err(err) => {
|
||
eprintln!("[lesavka-uvc] open failed: {err:#}");
|
||
thread::sleep(Duration::from_millis(250));
|
||
continue;
|
||
}
|
||
};
|
||
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);
|
||
|
||
if let Err(err) = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_SETUP)
|
||
.and_then(|_| subscribe_event(fd, vidioc_subscribe, UVC_EVENT_DATA))
|
||
{
|
||
if should_retry_uvc_error(&err) {
|
||
eprintln!("[lesavka-uvc] subscribe failed: {err:#}");
|
||
thread::sleep(Duration::from_millis(250));
|
||
continue;
|
||
}
|
||
return Err(err);
|
||
}
|
||
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_CONNECT).map_err(|err| {
|
||
eprintln!("[lesavka-uvc] connect-subscribe failed: {err:#}");
|
||
});
|
||
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_DISCONNECT).map_err(|err| {
|
||
eprintln!("[lesavka-uvc] disconnect-subscribe failed: {err:#}");
|
||
});
|
||
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_STREAMON).map_err(|err| {
|
||
eprintln!("[lesavka-uvc] streamon-subscribe failed: {err:#}");
|
||
});
|
||
let _ = subscribe_event(fd, vidioc_subscribe, UVC_EVENT_STREAMOFF).map_err(|err| {
|
||
eprintln!("[lesavka-uvc] streamoff-subscribe failed: {err:#}");
|
||
});
|
||
|
||
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(
|
||
fd,
|
||
uvc_send_response,
|
||
&mut state,
|
||
&mut pending,
|
||
interfaces,
|
||
data,
|
||
debug,
|
||
);
|
||
}
|
||
_ => {
|
||
if debug {
|
||
eprintln!("[lesavka-uvc] event type=0x{:08x}", ev.type_);
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
fn should_retry_uvc_error(err: &anyhow::Error) -> bool {
|
||
err.chain()
|
||
.find_map(|cause| {
|
||
cause
|
||
.downcast_ref::<std::io::Error>()
|
||
.and_then(|error| error.raw_os_error())
|
||
})
|
||
.is_some_and(|code| {
|
||
matches!(
|
||
code,
|
||
libc::ENOENT | libc::ENODEV | libc::EIO | libc::EBADF | libc::EAGAIN
|
||
)
|
||
})
|
||
}
|
||
|
||
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(),
|
||
_ if arg.starts_with('/') => 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);
|
||
let frame_size = env_u32("LESAVKA_UVC_FRAME_SIZE", width * height * 2);
|
||
let bulk = env::var("LESAVKA_UVC_BULK").is_ok();
|
||
if let Some(cap) = compute_payload_cap(bulk) {
|
||
if max_packet > cap.limit {
|
||
eprintln!(
|
||
"[lesavka-uvc] payload cap {}B ({}% from {}): clamp max_packet {} -> {} (periodic_dw={:?} non_periodic_dw={:?})",
|
||
cap.limit,
|
||
cap.pct,
|
||
cap.source,
|
||
max_packet,
|
||
cap.limit,
|
||
cap.periodic_dw,
|
||
cap.non_periodic_dw
|
||
);
|
||
max_packet = cap.limit;
|
||
} else {
|
||
eprintln!(
|
||
"[lesavka-uvc] payload cap {}B ({}% from {}): max_packet {} (periodic_dw={:?} non_periodic_dw={:?})",
|
||
cap.limit,
|
||
cap.pct,
|
||
cap.source,
|
||
max_packet,
|
||
cap.periodic_dw,
|
||
cap.non_periodic_dw
|
||
);
|
||
}
|
||
} else {
|
||
eprintln!(
|
||
"[lesavka-uvc] payload cap unavailable; using max_packet {}",
|
||
max_packet
|
||
);
|
||
}
|
||
if let Some(cfg_max) = read_u32_file(&format!("{CONFIGFS_UVC_BASE}/streaming_maxpacket")) {
|
||
if max_packet > cfg_max {
|
||
eprintln!(
|
||
"[lesavka-uvc] configfs maxpacket {}: clamp max_packet {} -> {}",
|
||
cfg_max, max_packet, cfg_max
|
||
);
|
||
max_packet = cfg_max;
|
||
} else {
|
||
eprintln!(
|
||
"[lesavka-uvc] configfs maxpacket {}: max_packet {}",
|
||
cfg_max, max_packet
|
||
);
|
||
}
|
||
}
|
||
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,
|
||
frame_size,
|
||
}
|
||
}
|
||
}
|
||
|
||
impl UvcState {
|
||
fn new(cfg: UvcConfig) -> Self {
|
||
let _profile_hint = (cfg.width, cfg.height, cfg.fps);
|
||
let ctrl_len = stream_ctrl_len();
|
||
let default = build_streaming_control(&cfg, ctrl_len);
|
||
Self {
|
||
cfg,
|
||
ctrl_len,
|
||
default,
|
||
probe: default,
|
||
commit: default,
|
||
cfg_snapshot: None,
|
||
}
|
||
}
|
||
}
|
||
|
||
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> {
|
||
let nonblock = env::var("LESAVKA_UVC_BLOCKING").is_err();
|
||
for attempt in 1..=200 {
|
||
let mut opts = OpenOptions::new();
|
||
opts.read(true).write(true);
|
||
if nonblock {
|
||
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 selector = (req.w_value >> 8) as u8;
|
||
let interface_lo = (req.w_index & 0xff) as u8;
|
||
let interface_hi = (req.w_index >> 8) as u8;
|
||
let interface_raw =
|
||
if interface_hi == interfaces.streaming || interface_hi == interfaces.control {
|
||
interface_hi
|
||
} else if interface_lo == interfaces.streaming || interface_lo == interfaces.control {
|
||
interface_lo
|
||
} else {
|
||
interface_hi
|
||
};
|
||
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 {
|
||
let len = req.w_length as usize;
|
||
if interface == interfaces.control {
|
||
// Accept SET_CUR on VC controls by replying with a zeroed payload.
|
||
let payload = vec![0u8; len.min(UVC_DATA_SIZE)];
|
||
let _ = send_response(fd, uvc_send_response, &payload);
|
||
if debug {
|
||
eprintln!(
|
||
"[lesavka-uvc] VC SET_CUR ack len={} iface={} sel={}",
|
||
req.w_length, interface, selector
|
||
);
|
||
}
|
||
return;
|
||
}
|
||
if interface != interfaces.streaming {
|
||
let _ = send_stall(fd, uvc_send_response);
|
||
return;
|
||
}
|
||
if len > UVC_DATA_SIZE {
|
||
eprintln!(
|
||
"[lesavka-uvc] SET_CUR too large len={} (max={}); stalling",
|
||
len, UVC_DATA_SIZE
|
||
);
|
||
let _ = send_stall(fd, uvc_send_response);
|
||
return;
|
||
}
|
||
*pending = Some(PendingRequest {
|
||
interface,
|
||
selector,
|
||
expected_len: len,
|
||
});
|
||
let payload = vec![0u8; len];
|
||
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) => {
|
||
if debug {
|
||
eprintln!(
|
||
"[lesavka-uvc] send IN response rq=0x{:02x} sel={} len={}",
|
||
req.b_request,
|
||
selector,
|
||
bytes.len()
|
||
);
|
||
}
|
||
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 {
|
||
let mapped = if selector == UVC_VS_PROBE_CONTROL {
|
||
interfaces.streaming
|
||
} else if selector == UVC_VS_COMMIT_CONTROL || selector == UVC_VC_REQUEST_ERROR_CODE_CONTROL {
|
||
if raw == interfaces.control {
|
||
interfaces.control
|
||
} else {
|
||
interfaces.streaming
|
||
}
|
||
} else {
|
||
raw
|
||
};
|
||
|
||
if debug && mapped != raw {
|
||
eprintln!("[lesavka-uvc] remapped interface {raw} -> {mapped} for selector {selector}");
|
||
}
|
||
|
||
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(
|
||
_fd: i32,
|
||
_uvc_send_response: libc::c_ulong,
|
||
state: &mut UvcState,
|
||
pending: &mut Option<PendingRequest>,
|
||
interfaces: UvcInterfaces,
|
||
data: UvcRequestData,
|
||
debug: bool,
|
||
) {
|
||
let Some(p) = pending.take() else {
|
||
if debug {
|
||
eprintln!("[lesavka-uvc] DATA with no pending request; ignoring");
|
||
}
|
||
return;
|
||
};
|
||
|
||
if data.length < 0 {
|
||
return;
|
||
}
|
||
|
||
let len = data.length as usize;
|
||
if debug && p.expected_len != 0 && len != p.expected_len {
|
||
eprintln!(
|
||
"[lesavka-uvc] DATA len mismatch: expected={} got={}",
|
||
p.expected_len, len
|
||
);
|
||
}
|
||
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
|
||
);
|
||
log_configfs_snapshot(state, "probe");
|
||
}
|
||
} 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
|
||
);
|
||
log_configfs_snapshot(state, "commit");
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
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]), // support GET/SET
|
||
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![0x03]), // indicate both GET/SET supported
|
||
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 {
|
||
// reset error code to “no error”
|
||
Some(vec![0x00])
|
||
} else {
|
||
// simple 1‑byte placeholder for unhandled VC controls
|
||
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];
|
||
|
||
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], cfg.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] = 0x03; // bmFramingInfo: FID + EOF supported
|
||
buf[31] = 0x01; // bPreferedVersion
|
||
buf[32] = 0x01; // bMinVersion
|
||
buf[33] = 0x01; // bMaxVersion
|
||
}
|
||
|
||
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_15 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 env_u32_opt(name: &str) -> Option<u32> {
|
||
env::var(name).ok().and_then(|v| v.parse::<u32>().ok())
|
||
}
|
||
|
||
fn read_u32_file(path: &str) -> Option<u32> {
|
||
std::fs::read_to_string(path)
|
||
.ok()
|
||
.and_then(|v| v.trim().parse::<u32>().ok())
|
||
}
|
||
|
||
fn read_u32_first(path: &str) -> Option<u32> {
|
||
std::fs::read_to_string(path)
|
||
.ok()
|
||
.and_then(|v| v.split_whitespace().next()?.parse::<u32>().ok())
|
||
}
|
||
|
||
fn read_configfs_snapshot() -> Option<ConfigfsSnapshot> {
|
||
let width = read_u32_file(&format!(
|
||
"{CONFIGFS_UVC_BASE}/streaming/mjpeg/m/720p/wWidth"
|
||
))?;
|
||
let height = read_u32_file(&format!(
|
||
"{CONFIGFS_UVC_BASE}/streaming/mjpeg/m/720p/wHeight"
|
||
))?;
|
||
let default_interval = read_u32_file(&format!(
|
||
"{CONFIGFS_UVC_BASE}/streaming/mjpeg/m/720p/dwDefaultFrameInterval"
|
||
))?;
|
||
let frame_interval = read_u32_first(&format!(
|
||
"{CONFIGFS_UVC_BASE}/streaming/mjpeg/m/720p/dwFrameInterval"
|
||
))
|
||
.unwrap_or(0);
|
||
let maxpacket = read_u32_file(&format!("{CONFIGFS_UVC_BASE}/streaming_maxpacket"))?;
|
||
let maxburst = read_u32_file(&format!("{CONFIGFS_UVC_BASE}/streaming_maxburst")).unwrap_or(0);
|
||
Some(ConfigfsSnapshot {
|
||
width,
|
||
height,
|
||
default_interval,
|
||
frame_interval,
|
||
maxpacket,
|
||
maxburst,
|
||
})
|
||
}
|
||
|
||
fn log_configfs_snapshot(state: &mut UvcState, label: &str) {
|
||
let Some(current) = read_configfs_snapshot() else {
|
||
eprintln!("[lesavka-uvc] configfs {label}: unavailable");
|
||
return;
|
||
};
|
||
if state.cfg_snapshot == Some(current) {
|
||
return;
|
||
}
|
||
eprintln!(
|
||
"[lesavka-uvc] configfs {label}: {}x{} default_interval={} frame_interval={} maxpacket={} maxburst={}",
|
||
current.width,
|
||
current.height,
|
||
current.default_interval,
|
||
current.frame_interval,
|
||
current.maxpacket,
|
||
current.maxburst
|
||
);
|
||
state.cfg_snapshot = Some(current);
|
||
}
|
||
|
||
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],
|
||
])
|
||
}
|
||
|
||
fn compute_payload_cap(bulk: bool) -> Option<PayloadCap> {
|
||
if let Some(limit) = env_u32_opt("LESAVKA_UVC_MAXPAYLOAD_LIMIT") {
|
||
return Some(PayloadCap {
|
||
limit,
|
||
pct: 100,
|
||
source: "env",
|
||
periodic_dw: None,
|
||
non_periodic_dw: None,
|
||
});
|
||
}
|
||
|
||
let mut periodic =
|
||
read_fifo_min("/sys/module/dwc2/parameters/g_tx_fifo_size").map(|v| (v, "dwc2.params"));
|
||
let mut non_periodic =
|
||
read_fifo_min("/sys/module/dwc2/parameters/g_np_tx_fifo_size").map(|v| (v, "dwc2.params"));
|
||
if (periodic.is_none() || non_periodic.is_none())
|
||
&& let Some((p, np)) = read_debugfs_fifos() {
|
||
if periodic.is_none() {
|
||
periodic = p.map(|v| (v, "debugfs.params"));
|
||
}
|
||
if non_periodic.is_none() {
|
||
non_periodic = np.map(|v| (v, "debugfs.params"));
|
||
}
|
||
}
|
||
let periodic_dw = periodic.map(|(v, _)| v);
|
||
let non_periodic_dw = non_periodic.map(|(v, _)| v);
|
||
|
||
let (fifo_dw, source) = if bulk {
|
||
if let Some((np, src)) = non_periodic {
|
||
(np, src)
|
||
} else if let Some((p, src)) = periodic {
|
||
(p, src)
|
||
} else {
|
||
return None;
|
||
}
|
||
} else if let Some((p, src)) = periodic {
|
||
(p, src)
|
||
} else if let Some((np, src)) = non_periodic {
|
||
(np, src)
|
||
} else {
|
||
return None;
|
||
};
|
||
|
||
let mut pct = env_u32("LESAVKA_UVC_LIMIT_PCT", 95);
|
||
if pct == 0 {
|
||
pct = 1;
|
||
} else if pct > 100 {
|
||
pct = 100;
|
||
}
|
||
let fifo_bytes = fifo_dw.saturating_mul(4);
|
||
let limit = fifo_bytes.saturating_mul(pct) / 100;
|
||
if limit == 0 {
|
||
return None;
|
||
}
|
||
|
||
Some(PayloadCap {
|
||
limit,
|
||
pct,
|
||
source,
|
||
periodic_dw,
|
||
non_periodic_dw,
|
||
})
|
||
}
|
||
|
||
fn read_fifo_min(path: &str) -> Option<u32> {
|
||
let raw = std::fs::read_to_string(path).ok()?;
|
||
raw.split(|c: char| c == ',' || c.is_whitespace())
|
||
.filter_map(|v| v.trim().parse::<u32>().ok())
|
||
.filter(|v| *v > 0)
|
||
.min()
|
||
}
|
||
|
||
fn read_debugfs_fifos() -> Option<(Option<u32>, Option<u32>)> {
|
||
let udc = std::fs::read_dir("/sys/class/udc")
|
||
.ok()?
|
||
.filter_map(|e| e.ok())
|
||
.filter_map(|e| e.file_name().into_string().ok())
|
||
.next()?;
|
||
let path = format!("/sys/kernel/debug/usb/{udc}/params");
|
||
let text = std::fs::read_to_string(path).ok()?;
|
||
let mut periodic: Option<u32> = None;
|
||
let mut non_periodic: Option<u32> = None;
|
||
for line in text.lines() {
|
||
let mut parts = line.splitn(2, ':');
|
||
let key = match parts.next() {
|
||
Some(v) => v.trim(),
|
||
None => continue,
|
||
};
|
||
let val = match parts.next().and_then(|v| v.trim().parse::<u32>().ok()) {
|
||
Some(v) => v,
|
||
None => continue,
|
||
};
|
||
if key == "g_np_tx_fifo_size" {
|
||
non_periodic = Some(val);
|
||
} else if key.starts_with("g_tx_fifo_size[") && val > 0 {
|
||
periodic = Some(match periodic {
|
||
Some(prev) => prev.min(val),
|
||
None => val,
|
||
});
|
||
}
|
||
}
|
||
if periodic.is_none() && non_periodic.is_none() {
|
||
None
|
||
} else {
|
||
Some((periodic, non_periodic))
|
||
}
|
||
}
|
||
|
||
const IOC_NRBITS: u8 = 8;
|
||
const IOC_TYPEBITS: u8 = 8;
|
||
const IOC_SIZEBITS: u8 = 14;
|
||
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
|
||
}
|