493 lines
15 KiB
Rust
493 lines
15 KiB
Rust
use std::env;
|
|
|
|
pub(crate) const STREAM_CTRL_SIZE_11: usize = 26;
|
|
pub(crate) const STREAM_CTRL_SIZE_15: usize = 34;
|
|
pub(crate) const STREAM_CTRL_SIZE_MAX: usize = STREAM_CTRL_SIZE_15;
|
|
pub(crate) const UVC_DATA_SIZE: usize = 60;
|
|
|
|
pub(crate) const V4L2_EVENT_PRIVATE_START: u32 = 0x0800_0000;
|
|
pub(crate) const UVC_EVENT_CONNECT: u32 = V4L2_EVENT_PRIVATE_START + 0;
|
|
pub(crate) const UVC_EVENT_DISCONNECT: u32 = V4L2_EVENT_PRIVATE_START + 1;
|
|
pub(crate) const UVC_EVENT_STREAMON: u32 = V4L2_EVENT_PRIVATE_START + 2;
|
|
pub(crate) const UVC_EVENT_STREAMOFF: u32 = V4L2_EVENT_PRIVATE_START + 3;
|
|
pub(crate) const UVC_EVENT_SETUP: u32 = V4L2_EVENT_PRIVATE_START + 4;
|
|
pub(crate) const UVC_EVENT_DATA: u32 = V4L2_EVENT_PRIVATE_START + 5;
|
|
|
|
pub(crate) const UVC_STRING_CONTROL_IDX: u8 = 0;
|
|
pub(crate) const UVC_STRING_STREAMING_IDX: u8 = 1;
|
|
|
|
pub(crate) const CONFIGFS_UVC_BASE: &str =
|
|
"/sys/kernel/config/usb_gadget/lesavka/functions/uvc.usb0";
|
|
|
|
pub(crate) const USB_DIR_IN: u8 = 0x80;
|
|
|
|
pub(crate) const UVC_SET_CUR: u8 = 0x01;
|
|
pub(crate) const UVC_GET_CUR: u8 = 0x81;
|
|
pub(crate) const UVC_GET_MIN: u8 = 0x82;
|
|
pub(crate) const UVC_GET_MAX: u8 = 0x83;
|
|
pub(crate) const UVC_GET_RES: u8 = 0x84;
|
|
pub(crate) const UVC_GET_LEN: u8 = 0x85;
|
|
pub(crate) const UVC_GET_INFO: u8 = 0x86;
|
|
pub(crate) const UVC_GET_DEF: u8 = 0x87;
|
|
|
|
pub(crate) const UVC_VS_PROBE_CONTROL: u8 = 0x01;
|
|
pub(crate) const UVC_VS_COMMIT_CONTROL: u8 = 0x02;
|
|
pub(crate) const UVC_VC_REQUEST_ERROR_CODE_CONTROL: u8 = 0x02;
|
|
|
|
#[repr(C)]
|
|
pub(crate) struct V4l2EventSubscription {
|
|
pub(crate) type_: u32,
|
|
pub(crate) id: u32,
|
|
pub(crate) flags: u32,
|
|
pub(crate) reserved: [u32; 5],
|
|
}
|
|
|
|
#[repr(C)]
|
|
pub(crate) union V4l2EventUnion {
|
|
pub(crate) data: [u8; 64],
|
|
pub(crate) _align: u64,
|
|
}
|
|
|
|
#[repr(C)]
|
|
pub(crate) struct V4l2Event {
|
|
pub(crate) type_: u32,
|
|
pub(crate) u: V4l2EventUnion,
|
|
pub(crate) pending: u32,
|
|
pub(crate) sequence: u32,
|
|
pub(crate) timestamp: libc::timespec,
|
|
pub(crate) id: u32,
|
|
pub(crate) reserved: [u32; 8],
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Clone, Copy)]
|
|
pub(crate) struct UsbCtrlRequest {
|
|
pub(crate) b_request_type: u8,
|
|
pub(crate) b_request: u8,
|
|
pub(crate) w_value: u16,
|
|
pub(crate) w_index: u16,
|
|
pub(crate) w_length: u16,
|
|
}
|
|
|
|
#[repr(C)]
|
|
#[derive(Clone, Copy)]
|
|
pub(crate) struct UvcRequestData {
|
|
pub(crate) length: i32,
|
|
pub(crate) data: [u8; UVC_DATA_SIZE],
|
|
}
|
|
|
|
#[derive(Clone, Copy)]
|
|
pub(crate) struct UvcConfig {
|
|
pub(crate) width: u32,
|
|
pub(crate) height: u32,
|
|
pub(crate) fps: u32,
|
|
pub(crate) interval: u32,
|
|
pub(crate) max_packet: u32,
|
|
pub(crate) frame_size: u32,
|
|
}
|
|
|
|
pub(crate) struct PayloadCap {
|
|
pub(crate) limit: u32,
|
|
pub(crate) pct: u32,
|
|
pub(crate) source: &'static str,
|
|
pub(crate) periodic_dw: Option<u32>,
|
|
pub(crate) non_periodic_dw: Option<u32>,
|
|
}
|
|
|
|
pub(crate) struct UvcState {
|
|
pub(crate) cfg: UvcConfig,
|
|
pub(crate) ctrl_len: usize,
|
|
pub(crate) default: [u8; STREAM_CTRL_SIZE_MAX],
|
|
pub(crate) probe: [u8; STREAM_CTRL_SIZE_MAX],
|
|
pub(crate) commit: [u8; STREAM_CTRL_SIZE_MAX],
|
|
pub(crate) cfg_snapshot: Option<ConfigfsSnapshot>,
|
|
}
|
|
|
|
#[derive(Clone, Copy)]
|
|
pub(crate) struct PendingRequest {
|
|
pub(crate) interface: u8,
|
|
pub(crate) selector: u8,
|
|
pub(crate) expected_len: usize,
|
|
}
|
|
|
|
#[derive(Clone, Copy)]
|
|
pub(crate) struct UvcInterfaces {
|
|
pub(crate) control: u8,
|
|
pub(crate) streaming: u8,
|
|
}
|
|
|
|
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
|
|
pub(crate) struct ConfigfsSnapshot {
|
|
pub(crate) width: u32,
|
|
pub(crate) height: u32,
|
|
pub(crate) default_interval: u32,
|
|
pub(crate) frame_interval: u32,
|
|
pub(crate) maxpacket: u32,
|
|
pub(crate) maxburst: u32,
|
|
}
|
|
|
|
impl UvcConfig {
|
|
pub(crate) 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", 30).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
|
|
);
|
|
}
|
|
}
|
|
max_packet = if env::var("LESAVKA_UVC_BULK").is_ok() {
|
|
max_packet.min(512)
|
|
} else {
|
|
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 {
|
|
pub(crate) 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,
|
|
cfg_snapshot: None,
|
|
}
|
|
}
|
|
}
|
|
|
|
pub(crate) 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,
|
|
}
|
|
}
|
|
|
|
pub(crate) fn env_u32(name: &str, default: u32) -> u32 {
|
|
env::var(name)
|
|
.ok()
|
|
.and_then(|value| value.parse::<u32>().ok())
|
|
.unwrap_or(default)
|
|
}
|
|
|
|
pub(crate) fn env_u8(name: &str) -> Option<u8> {
|
|
env::var(name).ok().and_then(|value| value.parse::<u8>().ok())
|
|
}
|
|
|
|
pub(crate) fn env_u32_opt(name: &str) -> Option<u32> {
|
|
env::var(name).ok().and_then(|value| value.parse::<u32>().ok())
|
|
}
|
|
|
|
pub(crate) fn read_u32_file(path: &str) -> Option<u32> {
|
|
std::fs::read_to_string(path)
|
|
.ok()
|
|
.and_then(|value| value.trim().parse::<u32>().ok())
|
|
}
|
|
|
|
pub(crate) fn read_u32_first(path: &str) -> Option<u32> {
|
|
std::fs::read_to_string(path)
|
|
.ok()
|
|
.and_then(|value| value.split_whitespace().next()?.parse::<u32>().ok())
|
|
}
|
|
|
|
pub(crate) fn read_configfs_snapshot() -> Option<ConfigfsSnapshot> {
|
|
let frame = configfs_frame_name_from_env();
|
|
let frame_root = format!("{CONFIGFS_UVC_BASE}/streaming/mjpeg/m/{frame}");
|
|
let width = read_u32_file(&format!("{frame_root}/wWidth"))?;
|
|
let height = read_u32_file(&format!("{frame_root}/wHeight"))?;
|
|
let default_interval = read_u32_file(&format!(
|
|
"{frame_root}/dwDefaultFrameInterval"
|
|
))?;
|
|
let frame_interval = read_u32_first(&format!("{frame_root}/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 configfs_frame_name_from_env() -> &'static str {
|
|
match (
|
|
env_u32("LESAVKA_UVC_WIDTH", 1280),
|
|
env_u32("LESAVKA_UVC_HEIGHT", 720),
|
|
) {
|
|
(1920, 1080) => "1080p",
|
|
_ => "720p",
|
|
}
|
|
}
|
|
|
|
pub(crate) 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);
|
|
}
|
|
|
|
pub(crate) 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
|
|
}
|
|
|
|
pub(crate) fn write_le16(dst: &mut [u8], val: u16) {
|
|
let bytes = val.to_le_bytes();
|
|
dst[0] = bytes[0];
|
|
dst[1] = bytes[1];
|
|
}
|
|
|
|
pub(crate) 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];
|
|
}
|
|
|
|
pub(crate) fn read_le32(src: &[u8], offset: usize) -> u32 {
|
|
u32::from_le_bytes([
|
|
src[offset],
|
|
src[offset + 1],
|
|
src[offset + 2],
|
|
src[offset + 3],
|
|
])
|
|
}
|
|
|
|
pub(crate) 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);
|
|
buf[2] = 1;
|
|
buf[3] = uvc_frame_index_for_mode(cfg.width, cfg.height);
|
|
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;
|
|
buf[31] = 0x01;
|
|
buf[32] = 0x01;
|
|
buf[33] = 0x01;
|
|
}
|
|
|
|
buf
|
|
}
|
|
|
|
pub(crate) fn uvc_frame_index_for_mode(width: u32, height: u32) -> u8 {
|
|
match (width, height) {
|
|
(1920, 1080) => 1,
|
|
(1280, 720) => 2,
|
|
_ => 2,
|
|
}
|
|
}
|
|
|
|
pub(crate) fn uvc_frame_index_for_request(requested: u8, cfg: &UvcConfig) -> u8 {
|
|
match requested {
|
|
1 | 2 => requested,
|
|
_ => uvc_frame_index_for_mode(cfg.width, cfg.height),
|
|
}
|
|
}
|
|
|
|
pub(crate) fn uvc_frame_size_for_index(frame_index: u8, fallback: u32) -> u32 {
|
|
match frame_index {
|
|
1 => 1920 * 1080 * 2,
|
|
2 => 1280 * 720 * 2,
|
|
_ => fallback,
|
|
}
|
|
}
|
|
|
|
pub(crate) 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(|value| (value, "dwc2.params"));
|
|
let mut non_periodic = read_fifo_min("/sys/module/dwc2/parameters/g_np_tx_fifo_size")
|
|
.map(|value| (value, "dwc2.params"));
|
|
if periodic.is_none() || non_periodic.is_none() {
|
|
if let Some((periodic_debug, non_periodic_debug)) = read_debugfs_fifos() {
|
|
if periodic.is_none() {
|
|
periodic = periodic_debug.map(|value| (value, "debugfs.params"));
|
|
}
|
|
if non_periodic.is_none() {
|
|
non_periodic = non_periodic_debug.map(|value| (value, "debugfs.params"));
|
|
}
|
|
}
|
|
}
|
|
let periodic_dw = periodic.map(|(value, _)| value);
|
|
let non_periodic_dw = non_periodic.map(|(value, _)| value);
|
|
|
|
let (fifo_dw, source) = if bulk {
|
|
if let Some((np, src)) = non_periodic {
|
|
(np, src)
|
|
} else if let Some((periodic_value, src)) = periodic {
|
|
(periodic_value, src)
|
|
} else {
|
|
return None;
|
|
}
|
|
} else if let Some((periodic_value, src)) = periodic {
|
|
(periodic_value, src)
|
|
} else if let Some((np, src)) = non_periodic {
|
|
(np, src)
|
|
} else {
|
|
return None;
|
|
};
|
|
|
|
let pct = env_u32("LESAVKA_UVC_LIMIT_PCT", 95).clamp(1, 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,
|
|
})
|
|
}
|
|
|
|
pub(crate) 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(|value| value.trim().parse::<u32>().ok())
|
|
.filter(|value| *value > 0)
|
|
.min()
|
|
}
|
|
|
|
pub(crate) fn read_debugfs_fifos() -> Option<(Option<u32>, Option<u32>)> {
|
|
let udc = std::fs::read_dir("/sys/class/udc")
|
|
.ok()?
|
|
.filter_map(|entry| entry.ok())
|
|
.filter_map(|entry| entry.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(value) => value.trim(),
|
|
None => continue,
|
|
};
|
|
let value = match parts.next().and_then(|raw| raw.trim().parse::<u32>().ok()) {
|
|
Some(value) => value,
|
|
None => continue,
|
|
};
|
|
if key == "g_np_tx_fifo_size" {
|
|
non_periodic = Some(value);
|
|
} else if key.starts_with("g_tx_fifo_size[") && value > 0 {
|
|
periodic = Some(match periodic {
|
|
Some(previous) => previous.min(value),
|
|
None => value,
|
|
});
|
|
}
|
|
}
|
|
if periodic.is_none() && non_periodic.is_none() {
|
|
None
|
|
} else {
|
|
Some((periodic, non_periodic))
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
#[path = "tests/model.rs"]
|
|
mod tests;
|