feat(f722): add usb mavlink transport

firmware/platforms/stm32f7/tests/usb_mavlink.rs

@@ -0,0 +1,434 @@
+use std::{
+    collections::VecDeque,
+    sync::{Arc, Mutex},
+};
+
+use kaptan_mavlink::{
+    MavlinkFrame,
+    encoder::MavlinkEncoder,
+    generated::kaptan::{FILE_DATA_DATA, MavMessage},
+};
+use kaptan_tx::{FrameClass, PendingFrame};
+use usb_device::{
+    UsbDirection, UsbError,
+    bus::{PollResult, UsbBus, UsbBusAllocator},
+    device::{UsbDeviceBuilder, UsbVidPid},
+    endpoint::{EndpointAddress, EndpointType},
+};
+use usbd_serial::CdcAcmClass;
+
+#[path = "../src/f722/usb.rs"]
+mod usb;
+use usb::UsbMavlink;
+
+// One pending packet per endpoint, with host-controlled completion. Device and
+// CDC control requests run through the real usb-device/usbd-serial code.
+#[derive(Default)]
+struct BusState {
+    events: VecDeque<PollResult>,
+    input: [Option<Vec<u8>>; 4],
+    output: [Option<Vec<u8>>; 4],
+    sizes: [[u16; 4]; 2],
+    stalled: [[bool; 4]; 2],
+    bulk_in: usize,
+    bulk_out: usize,
+}
+
+#[derive(Clone, Default)]
+struct Host(Arc<Mutex<BusState>>);
+
+struct PacketBus(Arc<Mutex<BusState>>);
+
+impl UsbBus for PacketBus {
+    fn alloc_ep(
+        &mut self,
+        direction: UsbDirection,
+        address: Option<EndpointAddress>,
+        kind: EndpointType,
+        size: u16,
+        _: u8,
+    ) -> Result<EndpointAddress, UsbError> {
+        let mut state = self.0.lock().unwrap();
+        let sizes = &mut state.sizes[usize::from(direction == UsbDirection::In)];
+        let index = match address {
+            Some(address) => address.index(),
+            None => (1..sizes.len())
+                .find(|index| sizes[*index] == 0)
+                .ok_or(UsbError::EndpointOverflow)?,
+        };
+        if index >= sizes.len() || sizes[index] != 0 {
+            return Err(UsbError::InvalidEndpoint);
+        }
+        sizes[index] = size;
+        if kind == EndpointType::Bulk {
+            match direction {
+                UsbDirection::In => state.bulk_in = index,
+                UsbDirection::Out => state.bulk_out = index,
+            }
+        }
+        Ok(EndpointAddress::from_parts(index, direction))
+    }
+
+    fn enable(&mut self) {}
+
+    fn reset(&self) {
+        let mut state = self.0.lock().unwrap();
+        state.input.fill(None);
+        state.output.fill(None);
+        state.events.clear();
+    }
+
+    fn set_device_address(&self, _: u8) {}
+
+    fn write(&self, address: EndpointAddress, bytes: &[u8]) -> Result<usize, UsbError> {
+        let mut state = self.0.lock().unwrap();
+        if bytes.len() > usize::from(state.sizes[1][address.index()]) {
+            return Err(UsbError::BufferOverflow);
+        }
+        let packet = &mut state.input[address.index()];
+        if packet.is_some() {
+            return Err(UsbError::WouldBlock);
+        }
+        *packet = Some(bytes.to_vec());
+        Ok(bytes.len())
+    }
+
+    fn read(&self, address: EndpointAddress, bytes: &mut [u8]) -> Result<usize, UsbError> {
+        let mut state = self.0.lock().unwrap();
+        let packet = state.output[address.index()]
+            .as_ref()
+            .ok_or(UsbError::WouldBlock)?;
+        if packet.len() > bytes.len() {
+            return Err(UsbError::BufferOverflow);
+        }
+        let packet = state.output[address.index()].take().unwrap();
+        bytes[..packet.len()].copy_from_slice(&packet);
+        Ok(packet.len())
+    }
+
+    fn set_stalled(&self, address: EndpointAddress, stalled: bool) {
+        self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()] = stalled;
+    }
+
+    fn is_stalled(&self, address: EndpointAddress) -> bool {
+        self.0.lock().unwrap().stalled[usize::from(address.is_in())][address.index()]
+    }
+
+    fn suspend(&self) {}
+    fn resume(&self) {}
+
+    fn poll(&self) -> PollResult {
+        self.0
+            .lock()
+            .unwrap()
+            .events
+            .pop_front()
+            .unwrap_or(PollResult::None)
+    }
+}
+
+impl Host {
+    fn event(&self, event: PollResult) {
+        self.0.lock().unwrap().events.push_back(event);
+    }
+
+    fn control(&self, link: &mut UsbMavlink<'_, PacketBus>, request: [u8; 8]) {
+        self.0.lock().unwrap().output[0] = Some(request.to_vec());
+        self.event(PollResult::Data {
+            ep_out: 0,
+            ep_in_complete: 0,
+            ep_setup: 1,
+        });
+        link.poll();
+        assert_eq!(self.take_input(0), Some(Vec::new()));
+        link.poll();
+    }
+
+    fn configure(&self, link: &mut UsbMavlink<'_, PacketBus>, configured: bool) {
+        self.control(link, [0, 9, u8::from(configured), 0, 0, 0, 0, 0]);
+    }
+
+    fn dtr(&self, link: &mut UsbMavlink<'_, PacketBus>, open: bool) {
+        self.control(link, [0x21, 0x22, u8::from(open), 0, 0, 0, 0, 0]);
+    }
+
+    fn write_packet(&self, bytes: &[u8]) {
+        assert!(bytes.len() <= 64);
+        let mut state = self.0.lock().unwrap();
+        let endpoint = state.bulk_out;
+        assert!(state.output[endpoint].is_none());
+        state.output[endpoint] = Some(bytes.to_vec());
+        state.events.push_back(PollResult::Data {
+            ep_out: 1 << endpoint,
+            ep_in_complete: 0,
+            ep_setup: 0,
+        });
+    }
+
+    fn take_input(&self, endpoint: usize) -> Option<Vec<u8>> {
+        let mut state = self.0.lock().unwrap();
+        let packet = state.input[endpoint].take()?;
+        state.events.push_back(PollResult::Data {
+            ep_out: 0,
+            ep_in_complete: 1 << endpoint,
+            ep_setup: 0,
+        });
+        Some(packet)
+    }
+
+    fn collect_packets(&self, link: &mut UsbMavlink<'_, PacketBus>) -> Vec<Vec<u8>> {
+        let endpoint = self.0.lock().unwrap().bulk_in;
+        let mut packets = Vec::new();
+        for _ in 0..16 {
+            let Some(packet) = self.take_input(endpoint) else {
+                return packets;
+            };
+            packets.push(packet);
+            link.poll();
+        }
+        panic!("transmission did not finish");
+    }
+}
+
+fn with_link(test: impl FnOnce(&Host, &mut UsbMavlink<'_, PacketBus>)) {
+    let host = Host::default();
+    let allocator = UsbBusAllocator::new(PacketBus(host.0.clone()));
+    let class = CdcAcmClass::new(&allocator, 64);
+    let device = UsbDeviceBuilder::new(&allocator, UsbVidPid(0x1209, 1))
+        .max_packet_size_0(64)
+        .unwrap()
+        .build();
+    let mut link = UsbMavlink::new(device, class);
+    host.event(PollResult::Reset);
+    link.poll();
+    host.configure(&mut link, true);
+    host.dtr(&mut link, true);
+    assert!(link.is_open());
+    test(&host, &mut link);
+}
+
+fn pending(frame: MavlinkFrame) -> PendingFrame<MavlinkFrame> {
+    PendingFrame {
+        class: FrameClass::Critical,
+        frame,
+    }
+}
+
+fn file_frame(length: usize) -> MavlinkFrame {
+    // FILE_DATA has nine scalar bytes before its trailing data array. Setting
+    // the final data byte controls MAVLink 2's trailing-zero truncation.
+    let mut data = FILE_DATA_DATA::DEFAULT;
+    data.data[..length - 12 - 9].fill(0x5a);
+    let frame = MavlinkEncoder::new(1, 1).encode(&data);
+    assert_eq!(frame.raw_bytes().len(), length);
+    frame
+}
+
+#[test]
+fn receives_split_frame_followed_by_coalesced_frames() {
+    with_link(|host, link| {
+        // HEARTBEAT ID 51027, source 255:1, sequence 0, empty payload,
+        // CRC-16/MCRF4XX with CRC_EXTRA 161. Integrity admission precedes payload
+        // validation, so these 12-byte frames must also survive USB batching.
+        let mut minimum = MavlinkFrame::new();
+        minimum.as_mut_slice()[..12]
+            .copy_from_slice(&[0xfd, 0, 0, 0, 0, 255, 1, 0x53, 0xc7, 0, 0x2e, 0xa8]);
+        assert!(minimum.has_valid_crc::<MavMessage>());
+        let mut frames = [minimum; 6];
+        frames[0] = MavlinkEncoder::new(255, 1).heartbeat();
+        let split = frames[0].raw_bytes().len() - 1;
+        host.write_packet(&frames[0].raw_bytes()[..split]);
+        assert!(link.poll());
+        assert!(link.take_received().is_none());
+
+        // One carried-over CRC byte and five complete frames: six completions
+        // in 61 bytes, exceeding the former five-frame receive queue.
+        let mut packet = frames[0].raw_bytes()[split..].to_vec();
+        for frame in &frames[1..] {
+            packet.extend_from_slice(frame.raw_bytes());
+        }
+        host.write_packet(&packet);
+        assert!(link.poll());
+        for frame in &frames {
+            assert_eq!(link.take_received().unwrap().raw(), frame);
+        }
+        assert!(link.take_received().is_none());
+    });
+}
+
+#[test]
+fn drains_endpoint_backlog_after_its_poll_event_was_consumed() {
+    with_link(|host, link| {
+        let mut encoder = MavlinkEncoder::new(255, 1);
+        let first = encoder.heartbeat();
+        let second = encoder.heartbeat();
+        host.write_packet(first.raw_bytes());
+        assert!(link.poll());
+        host.write_packet(second.raw_bytes());
+        assert!(link.poll());
+        assert_eq!(link.take_received().unwrap().raw(), &first);
+        assert!(link.take_received().is_none());
+
+        assert!(host.0.lock().unwrap().events.is_empty());
+        assert!(link.poll());
+        assert_eq!(link.take_received().unwrap().raw(), &second);
+    });
+}
+
+#[test]
+fn endpoint_backpressure_neither_repeats_nor_interleaves_frame_bytes() {
+    with_link(|host, link| {
+        let frame = file_frame(130);
+        let next = MavlinkEncoder::new(1, 1).heartbeat();
+        assert!(link.send(pending(frame)));
+        assert!(link.send(pending(next)));
+        for _ in 0..4 {
+            link.poll();
+        }
+        let packets = host.collect_packets(link);
+        assert_eq!(
+            packets.iter().map(Vec::len).collect::<Vec<_>>(),
+            [64, 64, 2, 13]
+        );
+        assert_eq!(
+            packets.concat(),
+            [frame.raw_bytes(), next.raw_bytes()].concat()
+        );
+    });
+}
+
+#[test]
+fn full_size_final_packet_is_followed_by_one_zlp() {
+    for length in [64, 128] {
+        with_link(|host, link| {
+            let frame = file_frame(length);
+            let next = MavlinkEncoder::new(1, 1).heartbeat();
+            assert!(link.send(pending(frame)));
+            assert!(link.send(pending(next)));
+            let packets = host.collect_packets(link);
+            let mut lengths = vec![64; length / 64];
+            lengths.extend([0, 13]);
+            assert_eq!(packets.iter().map(Vec::len).collect::<Vec<_>>(), lengths);
+            assert_eq!(
+                packets.concat(),
+                [frame.raw_bytes(), next.raw_bytes()].concat()
+            );
+        });
+    }
+}
+
+#[test]
+fn bounded_admission_preserves_reserved_frame_and_replaces_only_pending_frames() {
+    with_link(|host, link| {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let first = encoder.heartbeat();
+        let reserved = encoder.heartbeat();
+        assert!(link.send(pending(first)));
+        // The endpoint still owns the first packet; this start must retry.
+        assert!(link.send(pending(reserved)));
+        for _ in 0..4 {
+            assert!(link.send(PendingFrame {
+                class: FrameClass::BestEffort,
+                frame: encoder.heartbeat(),
+            }));
+        }
+        let critical = std::array::from_fn::<_, 4, _>(|_| encoder.heartbeat());
+        for frame in critical {
+            assert!(link.send(pending(frame)));
+        }
+        assert!(!link.send(pending(encoder.heartbeat())));
+        let vital = encoder.heartbeat();
+        assert!(link.send(PendingFrame {
+            class: FrameClass::Vital,
+            frame: vital,
+        }));
+
+        let packets = host.collect_packets(link);
+        assert_eq!(packets.len(), 6);
+        assert_eq!(packets[0], first.raw_bytes());
+        assert_eq!(packets[1], reserved.raw_bytes());
+        assert_eq!(packets[2], vital.raw_bytes());
+        let mut pending = critical.map(|frame| frame.raw_bytes().to_vec()).to_vec();
+        for packet in &packets[3..] {
+            let index = pending.iter().position(|frame| frame == packet).unwrap();
+            pending.remove(index);
+        }
+    });
+}
+
+#[test]
+fn dtr_suspend_and_deconfiguration_preserve_unfinished_transmission() {
+    enum Pause {
+        Dtr,
+        Suspend,
+        Deconfigure,
+    }
+    for pause in [Pause::Dtr, Pause::Suspend, Pause::Deconfigure] {
+        with_link(|host, link| {
+            let frame = file_frame(130);
+            assert!(link.send(pending(frame)));
+            match pause {
+                Pause::Dtr => host.dtr(link, false),
+                Pause::Suspend => {
+                    host.event(PollResult::Suspend);
+                    link.poll();
+                }
+                Pause::Deconfigure => host.configure(link, false),
+            }
+            assert!(!link.is_open());
+            assert!(!link.send(pending(MavlinkEncoder::new(1, 1).heartbeat())));
+            match pause {
+                Pause::Dtr => host.dtr(link, true),
+                Pause::Suspend => {
+                    host.event(PollResult::Resume);
+                    link.poll();
+                }
+                Pause::Deconfigure => host.configure(link, true),
+            }
+            assert!(link.is_open());
+            assert_eq!(host.collect_packets(link).concat(), frame.raw_bytes());
+        });
+    }
+}
+
+#[test]
+fn dtr_close_discards_partial_receive_frame() {
+    with_link(|host, link| {
+        let mut encoder = MavlinkEncoder::new(255, 1);
+        let interrupted = encoder.heartbeat();
+        let next = encoder.heartbeat();
+        host.write_packet(&interrupted.raw_bytes()[..8]);
+        link.poll();
+        assert!(link.take_received().is_none());
+        host.dtr(link, false);
+        host.dtr(link, true);
+        host.write_packet(next.raw_bytes());
+        link.poll();
+        assert_eq!(link.take_received().unwrap().raw(), &next);
+        assert!(link.take_received().is_none());
+    });
+}
+
+#[test]
+fn bus_reset_discards_transmit_and_receive_state() {
+    with_link(|host, link| {
+        let frame = file_frame(130);
+        assert!(link.send(pending(frame)));
+        host.write_packet(&frame.raw_bytes()[..8]);
+        link.poll();
+        assert!(link.take_received().is_none());
+        host.event(PollResult::Reset);
+        link.poll();
+        assert!(!link.is_open());
+        host.configure(link, true);
+        host.dtr(link, true);
+        assert!(host.collect_packets(link).is_empty());
+
+        let next = MavlinkEncoder::new(1, 1).heartbeat();
+        assert!(link.send(pending(next)));
+        assert_eq!(host.collect_packets(link).concat(), next.raw_bytes());
+        host.write_packet(next.raw_bytes());
+        link.poll();
+        assert_eq!(link.take_received().unwrap().raw(), &next);
+    });
+}