lesavka/server/src/uvc_control/model.rs

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;