kaptan/firmware/platforms/stm32f7/tests/support/io_latency.rs

//! Exercises the actual protocol worker and physical UART owners. Only elapsed
//! time, PAC registers and provider latency are simulated; no protocol is copied.
extern crate std;
use super::*;
use crate::f722::serial::{ReceiveEvent, SerialDevice, SerialIo, UartIo};
use crate::f722::{data_channel::exchange_usb, usb};
use kaptan_file_mavlink::{
    Content, ControlRequest, FileFailure, FileMetadata, FileOperation, FilePath, FileRequest,
    FileStatus,
};
use kaptan_mavlink::{encoder::MavlinkEncoder, receiver::IntegrityMavlinkFramer};
use std::{
    boxed::Box,
    cell::{Cell, RefCell},
    collections::VecDeque,
    rc::Rc,
    vec::Vec,
};
use usb::test_bus as usb_bus;

const INVENTORY: SerialInventory =
    SerialInventory::new(&[crate::UART1, crate::UART2, crate::UART4]);

#[derive(Default)]
struct Registers {
    rx: VecDeque<u8>,
    tx: Vec<u8>,
    tx_ready_irq: bool,
    tx_complete_irq: bool,
}
struct MockUart(Rc<RefCell<Registers>>);
impl UartIo for MockUart {
    fn tx_ready(&mut self) -> bool {
        true
    }
    fn tx_complete(&mut self) -> bool {
        true
    }
    fn write_byte(&mut self, byte: u8) {
        self.0.borrow_mut().tx.push(byte);
    }
    fn interrupts(&mut self, ready: bool, complete: bool) {
        let mut registers = self.0.borrow_mut();
        registers.tx_ready_irq = ready;
        registers.tx_complete_irq = complete;
    }
}
impl SerialDevice for MockUart {
    fn receive(&mut self) -> ReceiveEvent {
        self.0
            .borrow_mut()
            .rx
            .pop_front()
            .map_or(ReceiveEvent::Idle, ReceiveEvent::Byte)
    }
}

struct Io {
    uart: [SerialIo<MockUart>; 3],
    registers: [Rc<RefCell<Registers>>; 3],
    sent: Vec<(u8, kaptan::MavMessage)>,
    admitted: [usize; 3],
}
impl Io {
    fn new() -> Self {
        let registers = core::array::from_fn(|_| Rc::new(RefCell::new(Registers::default())));
        Self {
            uart: core::array::from_fn(|i| {
                SerialIo::new(Some(MockUart(registers[i].clone())), i != 2)
            }),
            registers,
            sent: Vec::new(),
            admitted: [0; 3],
        }
    }
    fn irq(&mut self, now: Timestamp) {
        for i in 0..3 {
            if !self.registers[i].borrow().rx.is_empty() {
                self.admitted[i] += 1;
            }
            self.uart[i].on_interrupt(now);
        }
    }
    fn finish_setup_transmit(&mut self, now: Timestamp) {
        // The owned TX queue has four pending slots plus reserved/active
        // storage. 2048 ready/complete IRQs cover even six maximum-sized frames.
        for _ in 0..2048 {
            let mut pending = false;
            for i in 0..2 {
                let enabled = {
                    let registers = self.registers[i].borrow();
                    registers.tx_ready_irq || registers.tx_complete_irq
                };
                if enabled {
                    pending = true;
                    self.uart[i].on_interrupt(now);
                }
            }
            if !pending {
                return;
            }
        }
        panic!("setup UART transmission did not reach quiescence");
    }
    fn exchange(&mut self, id: u8, frame: Option<PendingFrame<MavlinkFrame>>) -> IoBatch {
        if let Some(frame) = &frame {
            self.sent.push((id, checked(frame.frame).decode().unwrap()));
        }
        if id == 0 {
            return match frame {
                Some(_) => IoBatch {
                    accepted: true,
                    ..IoBatch::default()
                },
                None => IoBatch::default(),
            };
        }
        self.uart[INVENTORY.index(PortId::new(id)).unwrap()].exchange(frame)
    }
}

struct SlowProvider<'a> {
    work: Option<std::boxed::Box<dyn FnOnce() + 'a>>,
    opens: usize,
}
impl FileProvider for SlowProvider<'_> {
    fn open_read(&mut self, _: &FilePath) -> Result<FileMetadata, FileFailure> {
        self.opens += 1;
        if let Some(work) = self.work.take() {
            work();
        }
        Ok(FileMetadata {
            size: 0,
            checksum_crc32c: 0,
        })
    }
    fn open_write(&mut self, _: &FilePath, _: FileMetadata) -> Result<(), FileFailure> {
        unreachable!()
    }
    fn read(&mut self, _: u32, _: &mut [u8]) -> Result<(), FileFailure> {
        unreachable!()
    }
    fn write(&mut self, _: u32, _: &[u8]) -> Result<(), FileFailure> {
        unreachable!()
    }
    fn close(&mut self) -> Result<Content, FileFailure> {
        Ok(Content::EMPTY)
    }
    fn abort(&mut self) {}
}
fn checked(frame: MavlinkFrame) -> IntegrityCheckedFrame {
    let mut parser = IntegrityMavlinkFramer::new();
    frame
        .raw_bytes()
        .iter()
        .find_map(|&byte| parser.receive_byte(byte))
        .unwrap()
}
fn checked_stream(bytes: &[u8]) -> Vec<IntegrityCheckedFrame> {
    let mut parser = IntegrityMavlinkFramer::new();
    let frames: Vec<_> = bytes
        .iter()
        .filter_map(|&byte| parser.receive_byte(byte))
        .collect();
    assert_eq!(
        frames
            .iter()
            .map(|frame| frame.raw().raw_bytes().len())
            .sum::<usize>(),
        bytes.len(),
        "all transmitted bytes must belong to complete CRC-valid frames"
    );
    frames
}
fn open(system: u8) -> MavlinkFrame {
    let request = FileRequest::Control(ControlRequest {
        operation: FileOperation::OpenRead,
        target: Target::new(1, 1),
        position: 0,
        file_size: 0,
        checksum_crc32c: 0,
        transfer_id: 9,
        content: Content::try_new(b"/test").unwrap(),
    });
    let kaptan::MavMessage::FILE_CONTROL(data) = wire::encode_request(request) else {
        unreachable!()
    };
    MavlinkEncoder::new(system, 7).encode(&data)
}
fn mtf() -> [u8; 27] {
    let mut frame = [0; 27];
    frame[..6].copy_from_slice(&[0xef, 1, 1, 0x51, 0, 20]);
    frame[10..14].copy_from_slice(&1234u32.to_le_bytes());
    frame[16] = 1; // range valid
    frame[23] = 1; // flow valid
    frame[26] = frame[..26]
        .iter()
        .fold(0u8, |sum, byte| sum.wrapping_add(*byte));
    frame
}
fn worker(provider: SlowProvider<'_>) -> ProtocolWorker<SlowProvider<'_>, 3> {
    let mailboxes = Box::leak(Box::new((
        LatestImuMailbox::new(),
        LatestRangeMailbox::new(),
        LatestOpticalFlowMailbox::new(),
    )));
    let (_, imu) = mailboxes.0.split();
    let (range_publisher, range) = mailboxes.1.split();
    let (flow_publisher, optical_flow) = mailboxes.2.split();
    let rates = DataChannelRates {
        imu_hz: 1,
        heartbeat_hz: 1,
        range_hz: Some(98),
        optical_flow_hz: None,
    };
    ProtocolWorker {
        inventory: INVENTORY,
        usb: Link::new(1, 1, rates).unwrap(),
        serial: [
            Some(SerialEndpoint::Mavlink(Link::new(1, 1, rates).unwrap())),
            Some(SerialEndpoint::Mavlink(Link::new(1, 1, rates).unwrap())),
            Some(SerialEndpoint::Mtf01 {
                service: Mtf01::new(),
                range: range_publisher,
                optical_flow: flow_publisher,
            }),
        ],
        collector: DataCollector::new(DataSources {
            imu,
            range,
            optical_flow,
        }),
        config_file: provider,
        file_service: FileService::new(Target::new(1, 1)),
        file_lease: FileLease::default(),
        mavlink_errors: ErrorSink::new(),
        mavlink_logs: LogSinkQueue::new(crate::LevelFilter::Off),
    }
}

#[test]
fn compact_worker_routes_by_stable_id_and_never_exchanges_missing_ports() {
    let ProtocolWorker {
        usb,
        serial: [_, uart2, mtf01],
        collector,
        config_file,
        file_service,
        file_lease,
        mavlink_errors,
        mavlink_logs,
        ..
    } = worker(SlowProvider {
        work: None,
        opens: 0,
    });
    // Non-numeric order makes any residual platform-wide slot mapping fail.
    let mut worker = ProtocolWorker {
        usb,
        serial: [mtf01, uart2],
        inventory: SerialInventory::new(&[crate::UART4, crate::UART2]),
        collector,
        config_file,
        file_service,
        file_lease,
        mavlink_errors,
        mavlink_logs,
    };
    assert!(worker.link(1).is_none());
    assert!(worker.link(4).is_none());
    assert!(worker.link(2).is_some());
    let mut exchanges = Vec::new();
    worker.process_worker_events(
        2,
        |id, frame| {
            assert!(frame.is_none());
            exchanges.push(id);
            IoBatch::default()
        },
        Timestamp::default,
    );
    assert_eq!(exchanges, [4, 2, 0]);
    worker.dispatch_frame(
        2,
        checked(open(42)),
        &mut |id, frame| {
            assert_eq!(id, 2);
            assert!(matches!(
                checked(frame.unwrap().frame).decode(),
                Some(kaptan::MavMessage::FILE_CONTROL(_))
            ));
            IoBatch::default()
        },
        &mut Timestamp::default,
    );
    assert_eq!(worker.config_file.opens, 1);
}

#[test]
fn expensive_dispatch_keeps_uarts_and_usb_tx_serviceable_during_rx_backpressure() {
    usb_bus::with_link(|host, usb| {
        let usb = RefCell::new(usb);
        let io = Rc::new(RefCell::new(Io::new()));
        let time = Rc::new(Cell::new(0u64));
        let mut worker = worker(SlowProvider {
            work: None,
            opens: 0,
        });
        let mut exchange = |id, frame| {
            let batch = io.borrow_mut().exchange(id, frame);
            if id == 0 {
                exchange_usb(&mut usb.borrow_mut(), frame, |enabled| {
                    assert!(
                        usb.try_borrow_mut().is_err(),
                        "gate must finish inside the I/O borrow"
                    );
                    host.set_rx_interrupt_enabled(enabled)
                })
            } else {
                batch
            }
        };
        // Finish an initial sensor report and advance the real schedule to RANGE.
        io.borrow().registers[2].borrow_mut().rx.extend(mtf());
        for t in 1..=27 {
            io.borrow_mut().irq(Timestamp::from_microseconds(t * 100));
        }
        time.set(3000);
        for _ in 0..2 {
            worker.process_worker_events(1, &mut exchange, || {
                Timestamp::from_microseconds(time.get())
            });
        }
        assert_eq!(
            worker
                .collector
                .snapshot()
                .range
                .unwrap()
                .timestamp
                .as_microseconds(),
            2700
        );
        // Warm-up ticks may route process-global errors raised by another test.
        // Explicitly queue one diagnostic locally so the setup/backlog case is
        // deterministic without changing or draining the global error ingress.
        let mut diagnostic = kaptan::ERROR_DATA::DEFAULT;
        diagnostic.error_id = 65001;
        let raised_at = b"io_latency_setup:1";
        diagnostic.raised_at[..raised_at.len()].copy_from_slice(raised_at);
        for id in [0, 1, 2] {
            let frame = worker
                .link(id)
                .unwrap()
                .channel
                .encode_message(FrameClass::Critical, &diagnostic);
            assert!(exchange(id, Some(frame)).accepted);
        }
        // Settle setup traffic on every transport before queuing workload
        // markers. Vital traffic can overtake a pending critical diagnostic;
        // without this UART drain, that diagnostic could follow the marker.
        io.borrow_mut()
            .finish_setup_transmit(Timestamp::from_microseconds(time.get()));
        host.collect_packets(&mut usb.borrow_mut());
        let uart_tx_start: [usize; 2] = core::array::from_fn(|i| {
            let registers = io.borrow().registers[i].clone();
            let registers = registers.borrow();
            let frames = checked_stream(&registers.tx);
            assert!(frames.iter().any(|frame| matches!(
                frame.decode(), Some(kaptan::MavMessage::ERROR(data)) if data.error_id == 65001
            )));
            registers.tx.len()
        });
        io.borrow_mut().sent.clear();
        let heartbeat = MavlinkEncoder::new(1, 1).encode(&kaptan::HEARTBEAT_DATA::default());
        for id in [1, 2] {
            assert!(
                io.borrow_mut()
                    .exchange(
                        id,
                        Some(PendingFrame {
                            class: FrameClass::Vital,
                            frame: heartbeat,
                        }),
                    )
                    .accepted
            );
        }
        let mut data = kaptan::FILE_DATA_DATA::DEFAULT;
        data.data[..107].fill(0x5a);
        let usb_frame = MavlinkEncoder::new(1, 1).encode(&data);
        assert_eq!(usb_frame.raw_bytes().len(), 128);
        exchange_usb(
            &mut usb.borrow_mut(),
            Some(PendingFrame {
                class: FrameClass::Vital,
                frame: usb_frame,
            }),
            |enabled| host.set_rx_interrupt_enabled(enabled),
        );
        // Real MAVLink bytes on both UARTs and a second MTF report arrive during
        // expensive provider work. Borrowing I/O here would panic if dispatch still
        // ran under the caller's I/O lock.
        let during = io.clone();
        let clock = time.clone();
        let physical_usb = &usb;
        worker.config_file.work = Some(std::boxed::Box::new(move || {
            for i in 0..2 {
                during.borrow().registers[i]
                    .borrow_mut()
                    .rx
                    .extend(open(42).raw_bytes());
            }
            during.borrow().registers[2].borrow_mut().rx.extend(mtf());
            let mut transmitted_usb = Vec::new();
            for t in 1..=3000 {
                clock.set(3000 + t * 100);
                during
                    .borrow_mut()
                    .irq(Timestamp::from_microseconds(clock.get()));
                if t == 1 {
                    host.write_packet(open(42).raw_bytes());
                }
                if host.enter_irq() {
                    physical_usb.borrow_mut().with_rx_interrupt_gate(
                        |usb| usb.poll(),
                        |enabled| host.set_rx_interrupt_enabled(enabled),
                    );
                }
                if let Some(packet) = host.take_packet() {
                    transmitted_usb.push(packet);
                }
            }
            for (i, start) in uart_tx_start.into_iter().enumerate() {
                let registers = during.borrow().registers[i].clone();
                assert_eq!(
                    &registers.borrow().tx[start..],
                    heartbeat.raw_bytes(),
                    "UART {i} must transmit the complete workload marker during provider work"
                );
            }
            assert_eq!(
                transmitted_usb.concat(),
                usb_frame.raw_bytes(),
                "IN completion interrupts submit all remaining bytes during provider work"
            );
            assert_eq!(
                transmitted_usb.iter().map(Vec::len).collect::<Vec<_>>(),
                [64, 64, 0]
            );
            assert!(
                !host.rx_interrupt_enabled(),
                "only the RX FIFO interrupt waits for the consumer"
            );
            assert!(host.nvic_enabled());
            assert!(
                host.events_empty(),
                "TX completion was serviced without the worker"
            );
        }));
        host.write_packet(open(42).raw_bytes());
        worker.process_worker_events(1, &mut exchange, || {
            Timestamp::from_microseconds(time.get())
        });
        assert_eq!(time.get(), 303000);
        // The report that was fresh at worker entry became stale during dispatch.
        assert!(
            !io.borrow()
                .sent
                .iter()
                .any(|(_, msg)| matches!(msg, kaptan::MavMessage::RANGE(_)))
        );
        for _ in 0..32 {
            worker.process_worker_events(2, &mut exchange, || {
                Timestamp::from_microseconds(time.get())
            });
        }
        assert_eq!(
            worker
                .collector
                .snapshot()
                .range
                .unwrap()
                .timestamp
                .as_microseconds(),
            5700,
            "queued MTF bytes retain receipt time, not drain time"
        );
        assert_eq!(io.borrow().admitted[2], 54);
        assert!(
            host.rx_interrupt_enabled(),
            "draining RX rearms its FIFO interrupt after provider work"
        );
        for id in [0, 1, 2] {
            assert!(
                io.borrow().sent.iter().any(|(ingress, msg)| *ingress == id
                    && matches!(msg, kaptan::MavMessage::FILE_CONTROL(_))),
                "each link receives its own response"
            );
        }
        assert_eq!(
            worker.config_file.opens, 1,
            "USB retry received during provider work is replayed"
        );
        assert_eq!(
            io.borrow()
                .sent
                .iter()
                .filter(|(id, msg)| *id == 0 && matches!(msg, kaptan::MavMessage::FILE_CONTROL(_)))
                .count(),
            2
        );
        // A long silent interval followed by a new byte uses the SAME canonical time
        // source (serviced independently by TIM2), not a dormant per-sensor clock.
        time.set(60_000_000);
        io.borrow().registers[2].borrow_mut().rx.extend(mtf());
        for t in 1..=27 {
            io.borrow_mut()
                .irq(Timestamp::from_microseconds(time.get() + t));
        }
        for _ in 0..2 {
            worker.process_worker_events(2, &mut exchange, || {
                Timestamp::from_microseconds(time.get() + 100)
            });
        }
        assert_eq!(
            worker
                .collector
                .snapshot()
                .range
                .unwrap()
                .timestamp
                .as_microseconds(),
            60_000_027
        );
    });
}

#[test]
fn each_dispatch_rechecks_lease_and_foreign_requests_do_not_renew_it() {
    let mut worker = worker(SlowProvider {
        work: None,
        opens: 0,
    });
    let time = Cell::new(0u64);
    let mut replies = Vec::new();
    worker.dispatch_frame(
        0,
        checked(open(42)),
        &mut |id, frame| {
            replies.push((id, checked(frame.unwrap().frame).decode().unwrap()));
            IoBatch::default()
        },
        &mut || Timestamp::from_microseconds(time.get()),
    );
    assert_eq!(worker.file_lease.deadline, Some(10_000_000));
    let mut usb = Some(checked(open(43)));
    if let Some(SerialEndpoint::Mavlink(link)) = &mut worker.serial[0] {
        for &byte in open(42).raw_bytes() {
            link.receiver.receive_byte(byte);
        }
    }
    time.set(9_000_000);
    worker.process_worker_events(
        2,
        |id, frame| {
            if let Some(frame) = frame {
                replies.push((id, checked(frame.frame).decode().unwrap()));
                if id == 0 {
                    time.set(11_000_000);
                }
                IoBatch::default()
            } else if id == 0 {
                IoBatch {
                    usb: usb.take(),
                    ..IoBatch::default()
                }
            } else {
                IoBatch::default()
            }
        },
        || Timestamp::from_microseconds(time.get()),
    );
    let statuses: Vec<_> = replies
        .iter()
        .filter_map(|(id, msg)| match msg {
            kaptan::MavMessage::FILE_CONTROL(data) => Some((*id, data.status as u8)),
            _ => None,
        })
        .collect();
    assert_eq!(
        statuses,
        [
            (0, FileStatus::Ok as u8),
            (0, FileStatus::Busy as u8),
            (1, FileStatus::Ok as u8)
        ]
    );
    assert_eq!(worker.config_file.opens, 2);
    assert_eq!(worker.file_lease.deadline, Some(21_000_000));
}

#[test]
fn level_triggered_usb_and_continuous_uart_notifications_reach_the_actual_consumer() {
    usb_bus::with_link(|host, usb| {
        let mut io = Io::new();
        let mut worker = worker(SlowProvider {
            work: None,
            opens: 0,
        });
        for _ in 0..3 {
            host.write_packet(open(42).raw_bytes());
        }
        let uart_frame = MavlinkEncoder::new(43, 7).heartbeat();
        let mut workers = 0;
        let mut usb_replies = 0;
        for tick in 0..128 {
            // Each decision has a genuine priority-14 byte arrival on both UARTs.
            // These notifications cannot let a repeated priority-8 USB level
            // prevent the priority-7 protocol consumer from running.
            for registers in &io.registers[..2] {
                registers
                    .borrow_mut()
                    .rx
                    .push_back(uart_frame.raw_bytes()[tick % uart_frame.raw_bytes().len()]);
            }
            let now = Timestamp::from_microseconds(tick as u64 * 100);
            io.irq(now);
            if host.enter_irq() {
                usb.with_rx_interrupt_gate(
                    |usb| usb.poll(),
                    |enabled| host.set_rx_interrupt_enabled(enabled),
                );
            } else {
                workers += 1;
                worker.process_worker_events(
                    2,
                    |id, frame| {
                        if id == 0 {
                            if frame.as_ref().is_some_and(|frame| {
                                matches!(
                                    checked(frame.frame).decode(),
                                    Some(kaptan::MavMessage::FILE_CONTROL(_))
                                )
                            }) {
                                usb_replies += 1;
                            }
                            exchange_usb(usb, frame, |enabled| {
                                host.set_rx_interrupt_enabled(enabled)
                            })
                        } else {
                            io.exchange(id, frame)
                        }
                    },
                    || now,
                );
            }
            host.take_packet(); // Host ACK, without a hidden device poll.
        }
        assert!(workers > 0);
        assert_eq!(worker.config_file.opens, 1);
        assert_eq!(
            usb_replies, 3,
            "the consumer decoded the request and replayed both retries"
        );
        assert_eq!(io.admitted[..2], [128, 128]);
        assert_eq!(host.receive_backlog(), (0, false));
        assert!(host.rx_interrupt_enabled());
        assert!(host.nvic_enabled());
    });
}