From 604a7aba8632309f33957df87554e1db85b1a0a2 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Onur=20=C3=96zkan?= Date: Sun, 6 Sep 2026 23:14:00 +0300 Subject: [PATCH] feat(f722): add usb mavlink transport --- firmware/Cargo.lock | 75 ++- firmware/libs/kaptan-data-channel/Cargo.toml | 2 +- firmware/libs/kaptan-data-channel/src/lib.rs | 165 +++---- firmware/libs/kaptan-mavlink/src/receiver.rs | 30 +- firmware/libs/kaptan-tx/src/lib.rs | 18 +- firmware/platforms/stm32f7/Cargo.toml | 4 + firmware/platforms/stm32f7/src/f722/board.rs | 123 +++-- firmware/platforms/stm32f7/src/f722/clock.rs | 2 + .../stm32f7/src/f722/data_channel.rs | 94 ++-- .../platforms/stm32f7/src/f722/firmware.rs | 11 + firmware/platforms/stm32f7/src/f722/mod.rs | 1 + .../stm32f7/src/f722/peripherals/clock.rs | 57 ++- .../stm32f7/src/f722/peripherals/gpio.rs | 17 + .../stm32f7/src/f722/peripherals/mod.rs | 4 +- .../stm32f7/src/f722/peripherals/usb.rs | 121 +++++ firmware/platforms/stm32f7/src/f722/usb.rs | 135 ++++++ .../stm32f7/tests/platform_safety_boundary.rs | 2 +- .../platforms/stm32f7/tests/usb_mavlink.rs | 434 ++++++++++++++++++ 18 files changed, 1087 insertions(+), 208 deletions(-) create mode 100644 firmware/platforms/stm32f7/src/f722/peripherals/usb.rs create mode 100644 firmware/platforms/stm32f7/src/f722/usb.rs create mode 100644 firmware/platforms/stm32f7/tests/usb_mavlink.rs diff --git a/firmware/Cargo.lock b/firmware/Cargo.lock index 7f357d8..fedf281 100644 --- a/firmware/Cargo.lock +++ b/firmware/Cargo.lock @@ -98,6 +98,12 @@ dependencies = [ "void", ] +[[package]] +name = "embedded-io" +version = "0.6.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "edd0f118536f44f5ccd48bcb8b111bdc3de888b58c74639dfb034a357d0f206d" + [[package]] name = "embedded-io" version = "0.7.1" @@ -110,7 +116,7 @@ version = "0.7.0" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "2564b9f813c544241430e147d8bc454815ef9ac998878d30cc3055449f7fd4c0" dependencies = [ - "embedded-io", + "embedded-io 0.7.1", ] [[package]] @@ -125,12 +131,31 @@ version = "0.3.4" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e4eba85ea1d0a966a983acd07deee566e67395d2d96b6fb39e62b5a833f1eb0b" +[[package]] +name = "hash32" +version = "0.3.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "47d60b12902ba28e2730cd37e95b8c9223af2808df9e902d4df49588d1470606" +dependencies = [ + "byteorder", +] + [[package]] name = "hashbrown" version = "0.17.1" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "ed5909b6e89a2db4456e54cd5f673791d7eca6732202bbf2a9cc504fe2f9b84a" +[[package]] +name = "heapless" +version = "0.8.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "0bfb9eb618601c89945a70e254898da93b13be0388091d42117462b265bb3fad" +dependencies = [ + "hash32", + "stable_deref_trait", +] + [[package]] name = "indexmap" version = "2.14.0" @@ -197,11 +222,11 @@ dependencies = [ name = "kaptan-data-channel" version = "0.1.0" dependencies = [ - "kaptan-dma", "kaptan-err-reportable", "kaptan-mavlink", "kaptan-sensor", "kaptan-time", + "kaptan-tx", "mavlink-core", ] @@ -408,7 +433,7 @@ checksum = "b31cc9f930c7edce0c1933659d405a330a28346cdf087bd283a56a6d72eb3d85" dependencies = [ "byteorder", "crc-any", - "embedded-io", + "embedded-io 0.7.1", "embedded-io-async", ] @@ -498,9 +523,13 @@ dependencies = [ "kaptan-sensor", "kaptan-spi", "kaptan-time", + "kaptan-tx", "panic-halt", "rtic", "stm32f7", + "synopsys-usb-otg", + "usb-device", + "usbd-serial", ] [[package]] @@ -672,6 +701,12 @@ dependencies = [ "serde_core", ] +[[package]] +name = "stable_deref_trait" +version = "1.2.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "6ce2be8dc25455e1f91df71bfa12ad37d7af1092ae736f3a6cd0e37bc7810596" + [[package]] name = "stm32f7" version = "0.16.0" @@ -705,6 +740,18 @@ dependencies = [ "unicode-ident", ] +[[package]] +name = "synopsys-usb-otg" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e948d523b316939545d8b21a48c27aef150ce25321b9f95ff7978647a806a6fe" +dependencies = [ + "cortex-m", + "embedded-hal", + "usb-device", + "vcell", +] + [[package]] name = "target-triple" version = "1.0.1" @@ -800,6 +847,28 @@ version = "1.0.24" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "e6e4313cd5fcd3dad5cafa179702e2b244f760991f45397d14d4ebf38247da75" +[[package]] +name = "usb-device" +version = "0.3.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "98816b1accafbb09085168b90f27e93d790b4bfa19d883466b5e53315b5f06a6" +dependencies = [ + "heapless", + "portable-atomic", +] + +[[package]] +name = "usbd-serial" +version = "0.2.2" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "065e4eaf93db81d5adac82d9cef8f8da314cb640fa7f89534b972383f1cf80fc" +dependencies = [ + "embedded-hal", + "embedded-io 0.6.1", + "nb 1.1.0", + "usb-device", +] + [[package]] name = "vcell" version = "0.1.3" diff --git a/firmware/libs/kaptan-data-channel/Cargo.toml b/firmware/libs/kaptan-data-channel/Cargo.toml index 0492ea0..b94e9e8 100644 --- a/firmware/libs/kaptan-data-channel/Cargo.toml +++ b/firmware/libs/kaptan-data-channel/Cargo.toml @@ -10,10 +10,10 @@ workspace = true [dependencies] kaptan-err-reportable = { path = "../kaptan-err-reportable" } -kaptan-dma = { path = "../kaptan-dma" } kaptan-mavlink = { path = "../kaptan-mavlink", default-features = false, features = [ "sensor-telemetry", ] } kaptan-sensor = { path = "../kaptan-sensor" } kaptan-time = { path = "../kaptan-time" } +kaptan-tx = { path = "../kaptan-tx" } mavlink-core = { version = "0.18", default-features = false } diff --git a/firmware/libs/kaptan-data-channel/src/lib.rs b/firmware/libs/kaptan-data-channel/src/lib.rs index 57bffea..494fd0a 100644 --- a/firmware/libs/kaptan-data-channel/src/lib.rs +++ b/firmware/libs/kaptan-data-channel/src/lib.rs @@ -1,21 +1,15 @@ -//! Complete outbound MAVLink data flow. +//! Outbound MAVLink frame production. //! //! This crate owns fixed-rate scheduling, mailbox consumption, vertical-data -//! freshness, MAVLink encoding, and bounded queue classification. A platform -//! supplies only mailbox references, vertical-data freshness, and a transport -//! sink. +//! freshness, MAVLink encoding, and frame classification. Platforms retain +//! transport ownership and decide where each produced frame is admitted. #![no_std] -use kaptan_dma::TxCompletion; use kaptan_err_reportable::KaptanError; -use kaptan_mavlink::{ - MavlinkFrame, - encoder::MavlinkEncoder, - generated::kaptan::MavMessage, - transmitter::{EnqueueResult, FrameClass}, -}; +use kaptan_mavlink::{MavlinkFrame, encoder::MavlinkEncoder, generated::kaptan::MavMessage}; use kaptan_sensor::{optical_flow::OpticalFlowSnapshot, range::RangeSnapshot}; +use kaptan_tx::{FrameClass, PendingFrame}; use mavlink_core::MessageData; mod mailbox; @@ -39,19 +33,6 @@ pub struct DataChannelRates { pub heartbeat_hz: usize, } -/// Platform transport used by a data channel. -/// -/// The channel decides exactly which encoded frame is admitted and with which -/// delivery class. The platform retains only DMA endpoint ownership. -pub trait FrameSink { - /// Admits one fully encoded frame to the platform's bounded transport. - fn send(&mut self, class: FrameClass, frame: MavlinkFrame) -> EnqueueResult; - - /// Applies one terminal DMA interrupt and starts the next retained frame - /// when possible. - fn on_dma_complete(&mut self, completion: TxCompletion); -} - /// Mailboxes read by one data-channel worker. #[derive(Clone, Copy)] pub struct DataSources<'a> { @@ -77,9 +58,8 @@ pub enum DataChannelError { ScheduleFull, } -/// Complete outbound data flow for one MAVLink endpoint. -pub struct DataChannel<'a, Sink, const SERVICE_HZ: usize> { - sink: Sink, +/// Scheduled outbound MAVLink frame producer. +pub struct DataChannel<'a, const SERVICE_HZ: usize> { encoder: MavlinkEncoder, schedule: Schedule, sources: DataSources<'a>, @@ -93,13 +73,9 @@ pub struct DataChannel<'a, Sink, const SERVICE_HZ: usize> { optical_flow_age_ticks: u8, } -impl<'a, Sink, const SERVICE_HZ: usize> DataChannel<'a, Sink, SERVICE_HZ> -where - Sink: FrameSink, -{ +impl<'a, const SERVICE_HZ: usize> DataChannel<'a, SERVICE_HZ> { /// Creates an outbound data flow from already-validated runtime inputs. pub fn new( - sink: Sink, system_id: u8, component_id: u8, rates: DataChannelRates, @@ -108,7 +84,6 @@ where ) -> Result { let schedule = Schedule::from_rates(rates)?; Ok(Self { - sink, encoder: MavlinkEncoder::new(system_id, component_id), schedule, sources, @@ -125,57 +100,53 @@ where /// Runs all outbound work due for one platform service tick. /// - /// No schedule token escapes this method. It refreshes cached vertical - /// observations, selects due data, encodes the MAVLink message, and admits - /// it with the correct bounded-queue class. - pub fn on_service_tick(&mut self) { + /// At most one classified frame is returned for transport admission. + pub fn on_service_tick(&mut self) -> Option> { self.refresh_vertical_data(); match self.schedule.next_due() { Some(DueData::Imu) => { - if let Some(sample) = self.sources.imu.take_newer(&mut self.observed_imu) { - let _ = self - .sink - .send(FrameClass::BestEffort, self.encoder.imu_snapshot(&sample)); - } + self.sources + .imu + .take_newer(&mut self.observed_imu) + .map(|sample| PendingFrame { + class: FrameClass::BestEffort, + frame: self.encoder.imu_snapshot(&sample), + }) } Some(DueData::Distance) if self.range_age_ticks <= self.maximum_vertical_age_ticks => { - if let Some(range) = self.last_range { - let _ = self - .sink - .send(FrameClass::BestEffort, self.encoder.range_snapshot(&range)); - } + self.last_range.map(|range| PendingFrame { + class: FrameClass::BestEffort, + frame: self.encoder.range_snapshot(&range), + }) } Some(DueData::OpticalFlow) if self.optical_flow_age_ticks <= self.maximum_vertical_age_ticks => { - if let Some(flow) = self.last_optical_flow { - let _ = self.sink.send( - FrameClass::BestEffort, - self.encoder.optical_flow_snapshot(&flow), - ); - } + self.last_optical_flow.map(|flow| PendingFrame { + class: FrameClass::BestEffort, + frame: self.encoder.optical_flow_snapshot(&flow), + }) } - Some(DueData::Heartbeat) => { - let _ = self.sink.send(FrameClass::Vital, self.encoder.heartbeat()); - } - Some(DueData::Distance | DueData::OpticalFlow) | None => {} + Some(DueData::Heartbeat) => Some(PendingFrame { + class: FrameClass::Vital, + frame: self.encoder.heartbeat(), + }), + Some(DueData::Distance | DueData::OpticalFlow) | None => None, } } - /// Completes the active DMA lifecycle for this channel's transport. - pub fn on_dma_complete(&mut self, completion: TxCompletion) { - self.sink.on_dma_complete(completion) - } - - /// Encodes an adapter-owned generated MAVLink data record through the - /// channel's sole encoder and bounded transport sink. - pub fn send_message>( + /// Encodes an adapter-owned generated MAVLink record through the channel's + /// sole sequence-number owner. + pub fn encode_message>( &mut self, class: FrameClass, message: &M, - ) -> EnqueueResult { - self.sink.send(class, self.encoder.encode(message)) + ) -> PendingFrame { + PendingFrame { + class, + frame: self.encoder.encode(message), + } } fn refresh_vertical_data(&mut self) { @@ -276,39 +247,15 @@ impl Schedule { #[cfg(test)] mod tests { use super::{ - DataChannel, DataChannelRates, DataSources, FrameSink, LatestImuMailbox, - LatestOpticalFlowMailbox, LatestRangeMailbox, - }; - use kaptan_dma::TxCompletion; - use kaptan_mavlink::{ - MavlinkFrame, - transmitter::{EnqueueResult, FrameClass}, + DataChannel, DataChannelRates, DataSources, LatestImuMailbox, LatestOpticalFlowMailbox, + LatestRangeMailbox, }; + use kaptan_mavlink::generated::kaptan::MavMessage; use kaptan_sensor::{ imu::ImuSnapshot, optical_flow::OpticalFlowSnapshot, range::RangeSnapshot, }; use kaptan_time::Timestamp; - - struct Sink { - best_effort: u8, - critical: u8, - completions: u8, - } - - impl FrameSink for Sink { - fn send(&mut self, class: FrameClass, _: MavlinkFrame) -> EnqueueResult { - match class { - FrameClass::Vital => self.critical += 1, - FrameClass::BestEffort => self.best_effort += 1, - FrameClass::Critical => self.critical += 1, - } - EnqueueResult::Enqueued - } - - fn on_dma_complete(&mut self, _: TxCompletion) { - self.completions += 1; - } - } + use kaptan_tx::FrameClass; const RATES: DataChannelRates = DataChannelRates { imu_hz: 20, @@ -318,16 +265,11 @@ mod tests { }; #[test] - fn owns_rate_selection_encoding_admission_and_completion() { + fn owns_rate_selection_encoding_and_classification() { let imu = LatestImuMailbox::new(); let range = LatestRangeMailbox::new(); let flow = LatestOpticalFlowMailbox::new(); - let mut channel: DataChannel<'_, Sink, 100> = DataChannel::new( - Sink { - best_effort: 0, - critical: 0, - completions: 0, - }, + let mut channel: DataChannel<'_, 100> = DataChannel::new( 1, 1, RATES, @@ -339,6 +281,8 @@ mod tests { }, ) .unwrap(); + let mut best_effort = 0; + let mut vital = 0; for tick in 0..100 { imu.publish(ImuSnapshot::new( @@ -354,12 +298,17 @@ mod tests { 100, 100, )); - channel.on_service_tick(); + if let Some(frame) = channel.on_service_tick() { + assert!(frame.frame.has_valid_crc::()); + assert_eq!(frame.frame.sequence(), best_effort + vital); + match frame.class { + FrameClass::Vital => vital += 1, + FrameClass::BestEffort => best_effort += 1, + FrameClass::Critical => unreachable!(), + } + } } - channel.on_dma_complete(TxCompletion::Complete); - let sink = channel.sink; - assert_eq!(sink.best_effort, 60); - assert_eq!(sink.critical, 2); - assert_eq!(sink.completions, 1); + assert_eq!(best_effort, 60); + assert_eq!(vital, 2); } } diff --git a/firmware/libs/kaptan-mavlink/src/receiver.rs b/firmware/libs/kaptan-mavlink/src/receiver.rs index 4035654..17cbb35 100644 --- a/firmware/libs/kaptan-mavlink/src/receiver.rs +++ b/firmware/libs/kaptan-mavlink/src/receiver.rs @@ -5,7 +5,7 @@ use crate::{MavlinkFrame, decode_message, generated::kaptan::MavMessage}; /// An unsigned MAVLink 2 frame whose ID and CRC match the Kaptan dialect. /// /// The payload is deliberately not decoded during byte admission. Consumers -/// decode it once after taking the frame from the bounded receive queue. +/// decode it once at protocol dispatch. #[repr(transparent)] pub struct IntegrityCheckedFrame(MavlinkFrame); @@ -32,14 +32,20 @@ impl IntegrityCheckedFrame { } } -struct IntegrityFramer { +/// Incremental, unbuffered MAVLink 2 framing with Kaptan ID and CRC validation. +/// +/// Consumers that can pause byte admission can process each returned frame +/// directly. Interrupt-driven byte streams can use [`IntegrityMavlinkReceiver`] +/// to queue complete frames instead. Accepted frames remain unauthenticated. +pub struct IntegrityMavlinkFramer { frame: MavlinkFrame, received: usize, expected: usize, } -impl IntegrityFramer { - const fn new() -> Self { +impl IntegrityMavlinkFramer { + /// Creates an empty framer. + pub const fn new() -> Self { Self { frame: MavlinkFrame::new(), received: 0, @@ -47,7 +53,8 @@ impl IntegrityFramer { } } - fn receive_byte(&mut self, byte: u8) -> Option { + /// Consumes one byte, returning a complete integrity-checked frame if ready. + pub fn receive_byte(&mut self, byte: u8) -> Option { if self.received == 0 { if byte == 0xfd { self.frame.as_mut_slice()[0] = byte; @@ -81,20 +88,27 @@ impl IntegrityFramer { IntegrityCheckedFrame::try_new(frame) } - fn reset(&mut self) { + /// Discards a partial frame after a stream discontinuity. + pub fn reset(&mut self) { self.frame = MavlinkFrame::new(); self.received = 0; self.expected = 0; } } +impl Default for IntegrityMavlinkFramer { + fn default() -> Self { + Self::new() + } +} + /// Bounded integrity-only MAVLink 2 receiver. /// /// It rejects signed, incompatible, corrupt, and MAVLink 1 frames, but an /// accepted frame is still unauthenticated. The composition decides whether /// and how a received request may be admitted. pub struct IntegrityMavlinkReceiver { - framer: IntegrityFramer, + framer: IntegrityMavlinkFramer, frames: [Option; N], front: usize, length: usize, @@ -105,7 +119,7 @@ impl IntegrityMavlinkReceiver { pub const fn new() -> Self { assert!(N != 0, "integrity receiver capacity must be nonzero"); Self { - framer: IntegrityFramer::new(), + framer: IntegrityMavlinkFramer::new(), frames: [const { None }; N], front: 0, length: 0, diff --git a/firmware/libs/kaptan-tx/src/lib.rs b/firmware/libs/kaptan-tx/src/lib.rs index 5fe2ff8..0ccc497 100644 --- a/firmware/libs/kaptan-tx/src/lib.rs +++ b/firmware/libs/kaptan-tx/src/lib.rs @@ -1,7 +1,7 @@ //! Bounded protocol-neutral transmit admission and ownership lifecycle. //! //! Protocol encoders own frame bytes. This crate owns only bounded priority -//! admission and the transition from pending frame to active DMA ownership. +//! admission and the transition from pending frame to active transport ownership. #![no_std] @@ -9,7 +9,7 @@ #[derive(Clone, Copy, Debug, Eq, Ord, PartialEq, PartialOrd)] pub enum FrameClass { /// Heartbeat and current safety/health status. A pending vital frame may - /// displace any pending non-vital frame; active DMA storage is untouched. + /// displace any pending non-vital frame; active storage is untouched. Vital, /// Fault transitions and authoritative replies that best-effort samples /// cannot displace. @@ -147,12 +147,12 @@ impl TransmitQueue { Ok(StartOutcome::Started) } - /// Releases the active frame after a terminal DMA completion. + /// Releases the active frame after the transport no longer borrows it. pub fn complete(&mut self) -> Option> { self.active.take() } - /// Releases the active frame after a terminal DMA fault. + /// Releases the active frame after a terminal transport fault. pub fn fail(&mut self) -> Option> { self.active.take() } @@ -162,6 +162,14 @@ impl TransmitQueue { self.active.is_some() } + /// Borrows the frame currently owned by the active transport operation. + /// + /// Packetized transports use this to retain one selected frame while + /// advancing it across multiple bounded endpoint writes. + pub const fn active(&self) -> Option<&PendingFrame> { + self.active.as_ref() + } + fn take_next(&mut self) -> Option> { let vital = self .pending @@ -203,6 +211,7 @@ mod tests { Ok(StartOutcome::Started) ); assert!(queue.is_active()); + assert_eq!(queue.active().map(|pending| pending.frame), Some(1)); assert_eq!( queue.enqueue(PendingFrame { @@ -212,6 +221,7 @@ mod tests { EnqueueResult::Enqueued ); assert_eq!(queue.complete().unwrap().frame, 1); + assert!(queue.active().is_none()); } #[test] diff --git a/firmware/platforms/stm32f7/Cargo.toml b/firmware/platforms/stm32f7/Cargo.toml index d917036..8ba59e0 100644 --- a/firmware/platforms/stm32f7/Cargo.toml +++ b/firmware/platforms/stm32f7/Cargo.toml @@ -28,6 +28,7 @@ kaptan-mavlink = { path = "../../libs/kaptan-mavlink" } kaptan-sensor = { path = "../../libs/kaptan-sensor" } kaptan-spi = { path = "../../libs/kaptan-spi" } kaptan-time = { path = "../../libs/kaptan-time" } +kaptan-tx = { path = "../../libs/kaptan-tx" } cortex-m = "0.7" cortex-m-rt = "0.7" rtic = { version = "2.3", default-features = false, features = [ @@ -37,4 +38,7 @@ stm32f7 = { version = "0.16", default-features = false, features = [ "rt", "stm32f722", ] } +synopsys-usb-otg = { version = "0.4", features = ["cortex-m", "fs"] } +usb-device = "0.3" +usbd-serial = "0.2" panic-halt = "1" diff --git a/firmware/platforms/stm32f7/src/f722/board.rs b/firmware/platforms/stm32f7/src/f722/board.rs index 0444b33..898d551 100644 --- a/firmware/platforms/stm32f7/src/f722/board.rs +++ b/firmware/platforms/stm32f7/src/f722/board.rs @@ -7,7 +7,7 @@ use super::peripherals::{ configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9, configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12, configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, - configure_usart2_pins_pa2_pa3, + configure_usart2_pins_pa2_pa3, configure_usb_fs_pins_pa11_pa12, }; use kaptan_config::Config; use kaptan_data_channel::{ @@ -40,7 +40,8 @@ use super::config::{ use super::{ VerticalRoute, clock, data_channel::{ - F722MavlinkTransmitter, F722StatusLed, GeprcStatusLed, SERVICE_HZ, SERVICE_TIMER_CONFIG, + F722MavlinkLink, F722StatusLed, GeprcStatusLed, MavlinkIngress, SERVICE_HZ, + SERVICE_TIMER_CONFIG, }, interrupts, sensors::{F722Imu, ImuEvent}, @@ -80,17 +81,19 @@ pub type F722ServiceTimer = Tim2Periodic; /// RTIC-shared MAVLink state. /// -/// Only the MAVLink worker accesses this state, so its priority ceiling is 9 -/// rather than the IMU or vertical-sensor IRQ ceiling. +/// The UART and USB handlers do bounded byte/packet admission at priority 8; +/// protocol dispatch and transmission policy remain in the priority-9 worker. +/// This state therefore cannot inherit the IMU or vertical-sensor IRQ ceiling. pub struct F722MavlinkState where C: Clone + Config, I: F7ImmutableConfig, { - data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>, + data_channel: DataChannel<'static, SERVICE_HZ>, + link: F722MavlinkLink, config_file: F722ConfigFile, file_service: FileService, - receiver: IntegrityMavlinkReceiver<1>, + uart_receiver: IntegrityMavlinkReceiver<1>, file_lease_ticks: u16, mavlink_errors: ErrorSink, mavlink_logs: LogSinkQueue<1, 1>, @@ -229,9 +232,16 @@ where .mpu6000_config() .ok_or(BoardError::Initialization)?, ); - let transmitter = - F722MavlinkTransmitter::usart1(&device.RCC, device.USART1, hardware.mavlink_baud()) - .map_err(|_| BoardError::Initialization)?; + let usb = initialize_usb( + &device.RCC, + &device.GPIOA, + device.OTG_FS_GLOBAL, + device.OTG_FS_DEVICE, + device.OTG_FS_PWRCLK, + immutable.f722().name, + )?; + let link = F722MavlinkLink::usart1(&device.RCC, device.USART1, hardware.mavlink_baud(), usb) + .map_err(|_| BoardError::Initialization)?; if hardware.status_led() != F722StatusLedRoute::Pd2ActiveLow { return Err(BoardError::Initialization); } @@ -248,7 +258,7 @@ where range, mtf01, mtf01_uart, - transmitter, + link, led, configuration, ) @@ -299,9 +309,16 @@ where clock.fork(), hardware.imu_mount(), ); - let transmitter = - F722MavlinkTransmitter::usart2(&device.RCC, device.USART2, hardware.mavlink_baud()) - .map_err(|_| BoardError::Initialization)?; + let usb = initialize_usb( + &device.RCC, + &device.GPIOA, + device.OTG_FS_GLOBAL, + device.OTG_FS_DEVICE, + device.OTG_FS_PWRCLK, + immutable.f722().name, + )?; + let link = F722MavlinkLink::usart2(&device.RCC, device.USART2, hardware.mavlink_baud(), usb) + .map_err(|_| BoardError::Initialization)?; if hardware.status_led() != F722StatusLedRoute::Pa13ActiveHigh { return Err(BoardError::Initialization); } @@ -315,7 +332,7 @@ where range, mtf01, mtf01_uart, - transmitter, + link, led, configuration, ) @@ -331,7 +348,7 @@ fn finish( range: Option, mtf01: Option, mtf01_uart: Option, - transmitter: F722MavlinkTransmitter, + link: F722MavlinkLink, led: F722StatusLed, configuration: F722ConfigManager, ) -> Result, BoardError> @@ -350,7 +367,6 @@ where }; let mavlink_log_filter = runtime.mavlink_log_level_filter; let data_channel = DataChannel::new( - transmitter, system_id, component_id, rates, @@ -373,9 +389,10 @@ where }, mavlink: F722MavlinkState { data_channel, + link, config_file: F722ConfigFile::new(configuration), file_service: FileService::new(Target::new(system_id, component_id)), - receiver: IntegrityMavlinkReceiver::new(), + uart_receiver: IntegrityMavlinkReceiver::new(), file_lease_ticks: 0, mavlink_errors: ErrorSink::new(), mavlink_logs: LogSinkQueue::new(mavlink_log_filter), @@ -401,6 +418,19 @@ where .map_err(|_| BoardError::Configuration) } +fn initialize_usb( + rcc: &stm32f722::RCC, + gpioa: &stm32f722::GPIOA, + global: stm32f722::OTG_FS_GLOBAL, + device: stm32f722::OTG_FS_DEVICE, + power_clock: stm32f722::OTG_FS_PWRCLK, + product: &'static str, +) -> Result { + configure_usb_fs_pins_pa11_pa12(rcc, gpioa); + super::peripherals::usb_mavlink(global, device, power_clock, product) + .map_err(|_| BoardError::Initialization) +} + fn configure_vertical_pins( rcc: &stm32f722::RCC, gpioa: &stm32f722::GPIOA, @@ -583,13 +613,16 @@ where const RECEIVED: u8 = 1 << 1; const DMA: u8 = 1 << 2; - if events & RECEIVED != 0 { + // Poll at least every service tick, including when endpoint input was + // backpressured by a packet the worker had not yet consumed. + let usb_received = events & TIMER != 0 && self.link.poll_usb(); + if events & RECEIVED != 0 || usb_received { self.drain_file_frames(); } if events & DMA != 0 { let completion = take_mavlink_completion(); - self.data_channel.on_dma_complete(completion); + self.link.on_dma_complete(completion); if completion == kaptan_dma::TxCompletion::Complete { self.led.toggle(); } @@ -597,27 +630,41 @@ where if events & TIMER != 0 { self.tick_file_lease(); - self.data_channel.on_service_tick(); + if let Some(frame) = self.data_channel.on_service_tick() { + let _ = self.link.send_active(frame); + } self.drain_one_mavlink_diagnostic(); } } pub fn receive_mavlink_interrupt(&mut self, event: ReceiveInterrupt) -> bool { match event { - ReceiveInterrupt::Byte(byte) => self.receiver.receive_byte(byte), + ReceiveInterrupt::Byte(byte) => self.uart_receiver.receive_byte(byte), ReceiveInterrupt::Fault => { - self.receiver.reset(); - self.file_lease_ticks = 0; - self.file_service.expire(&mut self.config_file); - kaptan_log::warn!("mavlink receive fault; file transfer expired"); + self.uart_receiver.reset(); + kaptan_log::warn!("mavlink receive fault; framing reset"); false } ReceiveInterrupt::NotPending => false, } } + /// Acknowledges one OTG FS interrupt and admits USB MAVLink input. + pub fn on_usb_interrupt(&mut self) -> bool { + self.link.poll_usb() + } + fn drain_file_frames(&mut self) { - while let Some(frame) = self.receiver.take_received() { + while let Some((ingress, frame)) = self + .uart_receiver + .take_received() + .map(|frame| (MavlinkIngress::Uart, frame)) + .or_else(|| { + self.link + .take_usb_received() + .map(|frame| (MavlinkIngress::Usb, frame)) + }) + { let Some(principal) = Principal::try_new(frame.raw().system_id(), frame.raw().component_id()) else { @@ -644,7 +691,7 @@ where && control.status != FileStatus::RequestConflict } }; - self.send_file_response(response); + self.send_file_response(ingress, response); if refresh_lease { self.file_lease_ticks = FILE_TRANSFER_LEASE_TICKS; } @@ -661,15 +708,17 @@ where } } - fn send_file_response(&mut self, response: FileResponse) { + fn send_file_response(&mut self, ingress: MavlinkIngress, response: FileResponse) { let message = wire::encode_response(response); let class = kaptan_mavlink::transmitter::FrameClass::Critical; match message { kaptan::MavMessage::FILE_CONTROL(data) => { - let _ = self.data_channel.send_message(class, &data); + let frame = self.data_channel.encode_message(class, &data); + let _ = self.link.send_to(ingress, frame); } kaptan::MavMessage::FILE_DATA(data) => { - let _ = self.data_channel.send_message(class, &data); + let frame = self.data_channel.encode_message(class, &data); + let _ = self.link.send_to(ingress, frame); } _ => unreachable!("file response encoder returned another message"), } @@ -692,25 +741,25 @@ where } fn submit_pending_mavlink_error(&mut self) -> bool { - let data_channel = &mut self.data_channel; + let (data_channel, link) = (&mut self.data_channel, &mut self.link); self.mavlink_errors.submit_next(|record| { let payload = encode_error(record); - data_channel - .send_message(kaptan_mavlink::transmitter::FrameClass::Critical, &payload) - .is_retained() + let frame = data_channel + .encode_message(kaptan_mavlink::transmitter::FrameClass::Critical, &payload); + link.send_active(frame) }) } fn submit_queued_mavlink_log(&mut self) { - let data_channel = &mut self.data_channel; + let (data_channel, link) = (&mut self.data_channel, &mut self.link); let _ = self.mavlink_logs.submit_next(|record| { let class = match record.delivery() { DeliveryClass::Critical => kaptan_mavlink::transmitter::FrameClass::Critical, DeliveryClass::BestEffort => kaptan_mavlink::transmitter::FrameClass::BestEffort, }; let payload = encode_log(record); - let admission = data_channel.send_message(class, &payload); - let outcome = if admission.is_retained() { + let frame = data_channel.encode_message(class, &payload); + let outcome = if link.send_active(frame) { TransportAdmission::Accepted } else { TransportAdmission::Retry diff --git a/firmware/platforms/stm32f7/src/f722/clock.rs b/firmware/platforms/stm32f7/src/f722/clock.rs index ae24e4d..37bf3a1 100644 --- a/firmware/platforms/stm32f7/src/f722/clock.rs +++ b/firmware/platforms/stm32f7/src/f722/clock.rs @@ -6,6 +6,8 @@ use stm32f7::stm32f722; /// Core frequency in whole megahertz after selecting the PLL216 board profile. const CORE_CLOCK_MHZ: u32 = 216; +/// Core/AHB frequency used by fixed-frequency peripheral drivers. +pub(crate) const CORE_FREQUENCY_HZ: u32 = CORE_CLOCK_MHZ * 1_000_000; /// A failure while establishing the board's clock tree or monotonic clock. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] diff --git a/firmware/platforms/stm32f7/src/f722/data_channel.rs b/firmware/platforms/stm32f7/src/f722/data_channel.rs index d5d048d..8eea44f 100644 --- a/firmware/platforms/stm32f7/src/f722/data_channel.rs +++ b/firmware/platforms/stm32f7/src/f722/data_channel.rs @@ -1,12 +1,12 @@ //! F722 transport and vertical-route adapters for the portable data channel. use super::peripherals; -use kaptan_data_channel::FrameSink; use kaptan_dma::TxCompletion; use kaptan_mavlink::{ - MavlinkFrame, - transmitter::{EnqueueResult, FrameClass, MavlinkTransmitter}, + MavlinkFrame, receiver::IntegrityCheckedFrame, transmitter::MavlinkTransmitter, }; +use kaptan_tx::PendingFrame; +use peripherals::F722UsbMavlink; // Baud, sensor topology, and mounting remain immutable build-time policy. The // output-inhibited board image may recover endpoint identity and selected @@ -28,16 +28,25 @@ enum F722MavlinkTransport { Usart2(MavlinkTransmitter), } -/// Sole F722 MAVLink DMA owner. -pub(crate) struct F722MavlinkTransmitter { - inner: F722MavlinkTransport, +/// Physical link on which an inbound frame arrived. +#[derive(Clone, Copy, Debug, Eq, PartialEq)] +pub(crate) enum MavlinkIngress { + Uart, + Usb, } -impl F722MavlinkTransmitter { +/// Concrete F722 MAVLink link ownership. +pub(crate) struct F722MavlinkLink { + uart: F722MavlinkTransport, + usb: F722UsbMavlink, +} + +impl F722MavlinkLink { pub(crate) fn usart1( rcc: &stm32f7::stm32f722::RCC, uart: stm32f7::stm32f722::USART1, baud: u32, + usb: F722UsbMavlink, ) -> Result { let uart = peripherals::Usart1Tx::new(rcc, uart, UART1_CLOCK_HZ, baud).map_err(|_| ())?; // SAFETY: platform initialization disables D-cache before this object @@ -45,10 +54,11 @@ impl F722MavlinkTransmitter { // this platform receives a terminal DMA completion notification. let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() }; Ok(Self { - inner: F722MavlinkTransport::Usart1(MavlinkTransmitter::new( + uart: F722MavlinkTransport::Usart1(MavlinkTransmitter::new( memory, peripherals::Usart1TxDma::new(rcc, uart), )), + usb, }) } @@ -56,45 +66,73 @@ impl F722MavlinkTransmitter { rcc: &stm32f7::stm32f722::RCC, uart: stm32f7::stm32f722::USART2, baud: u32, + usb: F722UsbMavlink, ) -> Result { let uart = peripherals::Usart2Tx::new(rcc, uart, UART2_CLOCK_HZ, baud).map_err(|_| ())?; // SAFETY: identical DMA-memory invariant to the USART1 route above; // only the fixed platform DMA endpoint differs. let memory = unsafe { peripherals::DcacheDisabledDmaMemory::new() }; Ok(Self { - inner: F722MavlinkTransport::Usart2(MavlinkTransmitter::new( + uart: F722MavlinkTransport::Usart2(MavlinkTransmitter::new( memory, peripherals::Usart2TxDma::new(rcc, uart), )), + usb, }) } -} -impl FrameSink for F722MavlinkTransmitter { - fn send(&mut self, class: FrameClass, frame: MavlinkFrame) -> EnqueueResult { - match &mut self.inner { + /// Sends publications on the open USB link, or on UART when USB is closed. + pub(crate) fn send_active(&mut self, pending: PendingFrame) -> bool { + if self.usb.is_open() { + self.usb.send(pending) + } else { + self.send_uart(pending) + } + } + + /// Sends a protocol reply only on the link that supplied its request. + pub(crate) fn send_to( + &mut self, + ingress: MavlinkIngress, + pending: PendingFrame, + ) -> bool { + match ingress { + MavlinkIngress::Uart => self.send_uart(pending), + MavlinkIngress::Usb => self.usb.send(pending), + } + } + + fn send_uart(&mut self, pending: PendingFrame) -> bool { + match &mut self.uart { + F722MavlinkTransport::Usart1(transmitter) => transmitter + .admit_and_start(pending.class, pending.frame) + .admission + .is_retained(), + F722MavlinkTransport::Usart2(transmitter) => transmitter + .admit_and_start(pending.class, pending.frame) + .admission + .is_retained(), + } + } + + pub(crate) fn on_dma_complete(&mut self, completion: TxCompletion) { + match &mut self.uart { F722MavlinkTransport::Usart1(transmitter) => { - let outcome = transmitter.admit_and_start(class, frame); - let _ = outcome.start; - outcome.admission + let _ = transmitter.on_dma_complete(completion); } F722MavlinkTransport::Usart2(transmitter) => { - let outcome = transmitter.admit_and_start(class, frame); - let _ = outcome.start; - outcome.admission + let _ = transmitter.on_dma_complete(completion); } } } - fn on_dma_complete(&mut self, completion: TxCompletion) { - match &mut self.inner { - F722MavlinkTransport::Usart1(transmitter) => { - let _ = transmitter.on_dma_complete(completion); - } - F722MavlinkTransport::Usart2(transmitter) => { - let _ = transmitter.on_dma_complete(completion); - } - } + /// Services USB and reports whether the protocol worker has bytes to parse. + pub(crate) fn poll_usb(&mut self) -> bool { + self.usb.poll() + } + + pub(crate) fn take_usb_received(&mut self) -> Option { + self.usb.take_received() } } diff --git a/firmware/platforms/stm32f7/src/f722/firmware.rs b/firmware/platforms/stm32f7/src/f722/firmware.rs index 6842426..9e30917 100644 --- a/firmware/platforms/stm32f7/src/f722/firmware.rs +++ b/firmware/platforms/stm32f7/src/f722/firmware.rs @@ -248,6 +248,17 @@ macro_rules! __f722_flight_application { } } + #[task(binds = OTG_FS, priority = 8, shared = [mavlink])] + fn mavlink_usb(mut context: mavlink_usb::Context) { + if context + .shared + .mavlink + .lock(F722MavlinkState::on_usb_interrupt) + { + schedule_mavlink(MAVLINK_EVENT_RECEIVED); + } + } + #[task(binds = TIM2, priority = 4, local = [timer], shared = [lidar])] fn tim2(mut context: tim2::Context) { context.local.timer.acknowledge_update(); diff --git a/firmware/platforms/stm32f7/src/f722/mod.rs b/firmware/platforms/stm32f7/src/f722/mod.rs index 4bb6748..1df35fb 100644 --- a/firmware/platforms/stm32f7/src/f722/mod.rs +++ b/firmware/platforms/stm32f7/src/f722/mod.rs @@ -15,6 +15,7 @@ mod motor_inhibit; mod peripherals; mod routes; mod sensors; +mod usb; use kaptan_config::Config; diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs b/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs index 71fa1a7..00fe059 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/clock.rs @@ -3,7 +3,10 @@ use kaptan_err_reportable::KaptanError; use stm32f7::stm32f722; -const CLOCK_SWITCH_POLL_LIMIT: u16 = 1_024; +const CLOCK_SWITCH_POLL_LIMIT: u32 = 1_024; +// Crystal startup is slower than a clock-tree state transition. This remains +// bounded so an absent oscillator reaches safe-idle instead of hanging boot. +const HSE_STARTUP_POLL_LIMIT: u32 = 1_000_000; /// Failure to establish the 216 MHz PLL clock profile. #[derive(Clone, Copy, Debug, Eq, PartialEq, KaptanError)] @@ -22,30 +25,44 @@ pub enum ClockProfileError { OverDriveSwitchNotReady, #[kaptan_error(id = 11007)] SystemClockNotPll, + #[kaptan_error(id = 11008)] + HseNotReady, } -/// Selects the F722 platform's HSI PLL profile. +/// Selects the F722 platform's 8 MHz HSE PLL profile. /// /// SYSCLK/HCLK run at 216 MHz; APB1 is 54 MHz (timers run at 108 MHz), and -/// APB2 is 108 MHz. HSI is the PLL source, so no external-crystal assumption -/// is introduced. +/// APB2 is 108 MHz. Both supported flight-controller boards have an 8 MHz +/// external oscillator; using it as the PLL source gives USB an accurate +/// 48 MHz clock through PLLQ. pub fn select_pll216_profile( rcc: &stm32f722::RCC, pwr: &stm32f722::PWR, flash: &stm32f722::FLASH, ) -> Result<(), ClockProfileError> { rcc.cr().modify(|_, write| write.hsion().on()); - if !wait_until(|| rcc.cr().read().hsirdy().is_ready()) { + if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || { + rcc.cr().read().hsirdy().is_ready() + }) { return Err(ClockProfileError::HsiNotReady); } // Return to HSI before changing a PLL left configured by an earlier boot stage. rcc.cfgr().modify(|_, write| write.sw().hsi()); - if !wait_until(|| rcc.cfgr().read().sws().is_hsi()) { + if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || rcc.cfgr().read().sws().is_hsi()) { return Err(ClockProfileError::SystemClockNotHsi); } + + rcc.cr().modify(|_, write| write.hseon().on()); + if !wait_until(HSE_STARTUP_POLL_LIMIT, || { + rcc.cr().read().hserdy().is_ready() + }) { + return Err(ClockProfileError::HseNotReady); + } rcc.cr().modify(|_, write| write.pllon().off()); - if !wait_until(|| !rcc.cr().read().pllrdy().is_ready()) { + if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || { + !rcc.cr().read().pllrdy().is_ready() + }) { return Err(ClockProfileError::PllNotStopped); } @@ -56,11 +73,15 @@ pub fn select_pll216_profile( // over-drive-enable fields; it preserves every other PWR_CR1 bit owned by boot. unsafe { write.bits((read.bits() & !(0b11 << 14)) | (0b11 << 14) | (1 << 16)) } }); - if !wait_until(|| pwr.csr1().read().odrdy().bit_is_set()) { + if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || { + pwr.csr1().read().odrdy().bit_is_set() + }) { return Err(ClockProfileError::OverDriveNotReady); } pwr.cr1().modify(|_, write| write.odswen().set_bit()); - if !wait_until(|| pwr.csr1().read().odswrdy().bit_is_set()) { + if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || { + pwr.csr1().read().odswrdy().bit_is_set() + }) { return Err(ClockProfileError::OverDriveSwitchNotReady); } flash.acr().modify(|read, write| { @@ -69,14 +90,16 @@ pub fn select_pll216_profile( unsafe { write.bits((read.bits() & !0x0f) | 7 | (1 << 8) | (1 << 9)) } }); - // HSI / 16 * 432 / 2 = 216 MHz; Q=9 supplies a 48 MHz domain. + // HSE / 8 * 432 / 2 = 216 MHz; Q=9 supplies the exact 48 MHz USB domain. rcc.pllcfgr().write(|write| { - // SAFETY: 16, 432, P=/2 (the reset encoding), and Q=9 are the valid PLL - // divisors described above; the PLL is disabled before this full-register write. - unsafe { write.bits(16 | (432 << 6) | (9 << 24)) } + // SAFETY: M=8, N=432, P=/2 (the reset encoding), Q=9, and the HSE source + // are valid for the 8 MHz board oscillator; the PLL is disabled first. + unsafe { write.bits(8 | (432 << 6) | (1 << 22) | (9 << 24)) } }); rcc.cr().modify(|_, write| write.pllon().on()); - if !wait_until(|| rcc.cr().read().pllrdy().is_ready()) { + if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || { + rcc.cr().read().pllrdy().is_ready() + }) { return Err(ClockProfileError::PllNotReady); } rcc.cfgr().modify(|read, write| { @@ -92,14 +115,14 @@ pub fn select_pll216_profile( ) } }); - if !wait_until(|| rcc.cfgr().read().sws().is_pll()) { + if !wait_until(CLOCK_SWITCH_POLL_LIMIT, || rcc.cfgr().read().sws().is_pll()) { return Err(ClockProfileError::SystemClockNotPll); } Ok(()) } -fn wait_until(mut ready: impl FnMut() -> bool) -> bool { - for _ in 0..CLOCK_SWITCH_POLL_LIMIT { +fn wait_until(limit: u32, mut ready: impl FnMut() -> bool) -> bool { + for _ in 0..limit { if ready() { return true; } diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs b/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs index 70f9a0d..7df909d 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs @@ -174,6 +174,23 @@ pub fn configure_usart2_pins_pa2_pa3(port: &stm32f722::GPIOA) { .modify(|_, write| write.pupdr2().floating().pupdr3().floating()); } +/// Configures PA11/PA12 for the STM32F722 OTG FS data pair. +pub fn configure_usb_fs_pins_pa11_pa12(rcc: &stm32f722::RCC, port: &stm32f722::GPIOA) { + rcc.ahb1enr().modify(|_, write| write.gpioaen().set_bit()); + port.moder() + .modify(|_, write| write.moder11().alternate().moder12().alternate()); + port.afrh().modify(|_, write| { + // SAFETY: AF10 selects OTG_FS_DM/OTG_FS_DP on PA11/PA12. + unsafe { write.afrh11().bits(0b1010).afrh12().bits(0b1010) } + }); + port.otyper() + .modify(|_, write| write.ot11().push_pull().ot12().push_pull()); + port.ospeedr() + .modify(|_, write| write.ospeedr11().high_speed().ospeedr12().high_speed()); + port.pupdr() + .modify(|_, write| write.pupdr11().floating().pupdr12().floating()); +} + /// Configures PA0/PA1 for UART4 alternate function 8. pub fn configure_uart4_pins_pa0_pa1(port: &stm32f722::GPIOA) { port.moder() diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs b/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs index 789f846..2e09605 100644 --- a/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs +++ b/firmware/platforms/stm32f7/src/f722/peripherals/mod.rs @@ -14,6 +14,7 @@ mod time; mod usart; mod usart1_tx_dma2_stream7; mod usart2_tx_dma1_stream6; +mod usb; pub use clock::select_pll216_profile; pub use dma_buffer::DcacheDisabledDmaMemory; @@ -21,7 +22,7 @@ pub use gpio::{ ActiveLowLedPd2, configure_exti3_pin_pc3, configure_exti4_pin_pa4, configure_i2c1_pins_pb8_pb9, configure_spi1_pins_pa5_pa6_pa7, configure_spi3_pins_pc10_pc11_pc12, configure_uart4_pins_pa0_pa1, configure_usart1_rx_pin_pa10, configure_usart1_tx_pin_pa9, - configure_usart2_pins_pa2_pa3, + configure_usart2_pins_pa2_pa3, configure_usb_fs_pins_pa11_pa12, }; pub use i2c1::{I2c1, I2c1Config, I2c1Error}; pub use spi1::{Spi1, Spi1Error}; @@ -31,3 +32,4 @@ pub use time::DwtMicrosecondClock; pub use usart::{ReceiveInterrupt, Uart4, Usart1Tx, Usart2Tx}; pub use usart1_tx_dma2_stream7::Usart1TxDma; pub use usart2_tx_dma1_stream6::Usart2TxDma; +pub(crate) use usb::{F722UsbMavlink, usb_mavlink}; diff --git a/firmware/platforms/stm32f7/src/f722/peripherals/usb.rs b/firmware/platforms/stm32f7/src/f722/peripherals/usb.rs new file mode 100644 index 0000000..2f77472 --- /dev/null +++ b/firmware/platforms/stm32f7/src/f722/peripherals/usb.rs @@ -0,0 +1,121 @@ +//! STM32F722 OTG FS ownership and CDC-ACM initialization. + +use stm32f7::stm32f722; +use synopsys_usb_otg::{UsbBus, UsbPeripheral}; +use usb_device::{ + bus::UsbBusAllocator, + device::{StringDescriptors, UsbDeviceBuilder, UsbVidPid}, +}; +use usbd_serial::CdcAcmClass; + +use super::super::{ + clock::CORE_FREQUENCY_HZ, + usb::{USB_PACKET_BYTES, UsbMavlink}, +}; + +const USB_VID: u16 = 0x1209; +// pid.codes reserves 1209:0001 as a shared development/test identity. A +// project-specific allocated PID must replace it before product distribution. +const USB_PID: u16 = 0x0001; +// synopsys-usb-otg uses this RAM only for OUT endpoints. CDC-ACM has one +// 64-byte bulk OUT endpoint and EP0 has one 64-byte control OUT endpoint. +const ENDPOINT_MEMORY_WORDS: usize = (USB_PACKET_BYTES / 4) * 2; +const DEVICE_ID_ADDRESS: *const u32 = 0x1ff0_7a10 as *const u32; +const DEVICE_ID_BYTES: usize = 12; +const SERIAL_BYTES: usize = DEVICE_ID_BYTES * 2; + +/// Exclusive owner token for the split PAC views of one OTG FS controller. +pub(crate) struct F722UsbPeripheral { + _global: stm32f722::OTG_FS_GLOBAL, + _device: stm32f722::OTG_FS_DEVICE, + _power_clock: stm32f722::OTG_FS_PWRCLK, +} + +// SAFETY: the token is constructed once from PAC-owned peripherals and moved +// into one UsbBus. Runtime register access is synchronized by that bus. +unsafe impl Sync for F722UsbPeripheral {} + +// SAFETY: the F722 OTG FS register layout, FIFO depth, and endpoint count are +// the values required by synopsys-usb-otg for this device peripheral. +unsafe impl UsbPeripheral for F722UsbPeripheral { + const REGISTERS: *const () = stm32f722::OTG_FS_GLOBAL::ptr() as *const (); + const HIGH_SPEED: bool = false; + const FIFO_DEPTH_WORDS: usize = 320; + const ENDPOINT_COUNT: usize = 6; + + fn enable() { + cortex_m::interrupt::free(|_| { + // SAFETY: this static PAC view is used only for the USB clock/reset + // fields while the owned RCC token remains in platform init. + let rcc = unsafe { &*stm32f722::RCC::ptr() }; + rcc.dckcfgr2().modify(|_, write| write.ck48msel().pll()); + rcc.ahb2enr().modify(|_, write| write.otgfsen().set_bit()); + rcc.ahb2rstr().modify(|_, write| write.otgfsrst().set_bit()); + rcc.ahb2rstr() + .modify(|_, write| write.otgfsrst().clear_bit()); + }); + } + + fn ahb_frequency_hz(&self) -> u32 { + CORE_FREQUENCY_HZ + } +} + +type F722UsbBus = UsbBus; +type F722UsbBusAllocator = UsbBusAllocator; + +fn device_serial_number(serial: &'static mut [u8; SERIAL_BYTES]) -> Result<&'static str, ()> { + const HEX: &[u8; 16] = b"0123456789ABCDEF"; + for word_index in 0..3 { + // SAFETY: STM32F722 exposes a readable 96-bit factory device identifier + // at DEVICE_ID_ADDRESS. The singleton buffer is exclusively borrowed. + let word = unsafe { core::ptr::read_volatile(DEVICE_ID_ADDRESS.add(word_index)) }; + for byte_index in 0..4 { + let byte = (word >> (byte_index * 8)) as u8; + let output = (word_index * 4 + byte_index) * 2; + serial[output] = HEX[usize::from(byte >> 4)]; + serial[output + 1] = HEX[usize::from(byte & 0x0f)]; + } + } + core::str::from_utf8(serial).map_err(|_| ()) +} + +pub(crate) type F722UsbMavlink = UsbMavlink<'static, F722UsbBus>; + +/// Initializes the board's singleton CDC-ACM MAVLink endpoint. +pub(crate) fn usb_mavlink( + global: stm32f722::OTG_FS_GLOBAL, + device: stm32f722::OTG_FS_DEVICE, + power_clock: stm32f722::OTG_FS_PWRCLK, + product: &'static str, +) -> Result { + let peripheral = F722UsbPeripheral { + _global: global, + _device: device, + _power_clock: power_clock, + }; + let endpoint_memory = cortex_m::singleton!( + USB_ENDPOINT_MEMORY: [u32; ENDPOINT_MEMORY_WORDS] = [0; ENDPOINT_MEMORY_WORDS] + ) + .ok_or(())?; + let allocator = UsbBus::new(peripheral, endpoint_memory); + let allocator = cortex_m::singleton!( + USB_BUS_ALLOCATOR: F722UsbBusAllocator = allocator + ) + .ok_or(())?; + let serial_memory = + cortex_m::singleton!(USB_SERIAL: [u8; SERIAL_BYTES] = [0; SERIAL_BYTES]).ok_or(())?; + let serial_number = device_serial_number(serial_memory)?; + let cdc = CdcAcmClass::new(allocator, USB_PACKET_BYTES as u16); + let device = UsbDeviceBuilder::new(allocator, UsbVidPid(USB_VID, USB_PID)) + .strings(&[StringDescriptors::default() + .manufacturer("Kaptan") + .product(product) + .serial_number(serial_number)]) + .map_err(|_| ())? + .device_class(usbd_serial::USB_CLASS_CDC) + .max_packet_size_0(USB_PACKET_BYTES as u8) + .map_err(|_| ())? + .build(); + Ok(UsbMavlink::new(device, cdc)) +} diff --git a/firmware/platforms/stm32f7/src/f722/usb.rs b/firmware/platforms/stm32f7/src/f722/usb.rs new file mode 100644 index 0000000..653f7e4 --- /dev/null +++ b/firmware/platforms/stm32f7/src/f722/usb.rs @@ -0,0 +1,135 @@ +//! Bounded MAVLink byte stream over CDC-ACM bulk packets. + +use core::ops::Range; + +use kaptan_mavlink::{ + MavlinkFrame, + receiver::{IntegrityCheckedFrame, IntegrityMavlinkFramer}, +}; +use kaptan_tx::{PendingFrame, TransmitQueue}; +use usb_device::{ + UsbError, + bus::UsbBus, + device::{UsbDevice, UsbDeviceState}, +}; +use usbd_serial::CdcAcmClass; + +pub(crate) const USB_PACKET_BYTES: usize = 64; + +pub(crate) struct UsbMavlink<'a, B: UsbBus> { + device: UsbDevice<'a, B>, + cdc: CdcAcmClass<'a, B>, + transmit: TransmitQueue, + tx_offset: usize, + framer: IntegrityMavlinkFramer, + rx_packet: [u8; USB_PACKET_BYTES], + rx_remaining: Range, +} + +impl<'a, B: UsbBus> UsbMavlink<'a, B> { + pub(crate) fn new(device: UsbDevice<'a, B>, cdc: CdcAcmClass<'a, B>) -> Self { + Self { + device, + cdc, + transmit: TransmitQueue::new(), + tx_offset: 0, + framer: IntegrityMavlinkFramer::new(), + rx_packet: [0; USB_PACKET_BYTES], + rx_remaining: 0..0, + } + } + + pub(crate) fn is_open(&self) -> bool { + self.device.state() == UsbDeviceState::Configured && self.cdc.dtr() + } + + /// Returns whether the frame was retained, not whether the host received it. + pub(crate) fn send(&mut self, pending: PendingFrame) -> bool { + if !self.is_open() { + return false; + } + let retained = self.transmit.enqueue(pending).is_retained(); + self.service_transmit(); + retained + } + + /// Services USB and buffers at most one packet for the protocol worker. + pub(crate) fn poll(&mut self) -> bool { + let previous_state = self.device.state(); + let previous_dtr = self.cdc.dtr(); + self.device.poll(&mut [&mut self.cdc]); + + if previous_state != UsbDeviceState::Default + && self.device.state() == UsbDeviceState::Default + { + // Only bus reset flushes endpoint FIFOs. DTR close, suspend, and + // deconfiguration pause TX without invalidating its byte cursor. + self.transmit = TransmitQueue::new(); + self.tx_offset = 0; + self.reset_receive(); + } else if previous_dtr && !self.cdc.dtr() { + self.reset_receive(); + } + + if self.rx_remaining.is_empty() && self.device.state() == UsbDeviceState::Configured { + // A previous poll may have left a packet in the bus while our + // buffer was occupied. Reading must not depend on poll's result. + match self.cdc.read_packet(&mut self.rx_packet) { + Ok(length) if self.is_open() => self.rx_remaining = 0..length, + Ok(_) | Err(UsbError::WouldBlock) => {} + Err(error) => panic!("CDC OUT endpoint invariant: {error:?}"), + } + } + self.service_transmit(); + !self.rx_remaining.is_empty() + } + + /// Parses buffered bytes in the worker, without a second receive queue. + pub(crate) fn take_received(&mut self) -> Option { + for index in self.rx_remaining.by_ref() { + if let Some(frame) = self.framer.receive_byte(self.rx_packet[index]) { + return Some(frame); + } + } + None + } + + fn reset_receive(&mut self) { + self.framer.reset(); + self.rx_remaining = 0..0; + } + + fn service_transmit(&mut self) { + if !self.is_open() { + return; + } + let mut complete = false; + let offset = &mut self.tx_offset; + let cdc = &mut self.cdc; + let mut write = |frame: &MavlinkFrame| { + let bytes = frame.raw_bytes(); + let end = (*offset + USB_PACKET_BYTES).min(bytes.len()); + let packet = &bytes[*offset..end]; + let written = cdc.write_packet(packet)?; + assert_eq!(written, packet.len(), "CDC IN endpoint short write"); + *offset = end; + // An exact multiple of 64 needs a ZLP: retain offset == len until + // the next empty packet is accepted. The endpoint copies bytes, + // so a final short write ends its borrow of queue-owned storage. + complete = end == bytes.len() && packet.len() < USB_PACKET_BYTES; + Ok(()) + }; + let result = match self.transmit.active() { + Some(active) => write(&active.frame), + None => self.transmit.start_with(write).map(|_| ()), + }; + match result { + Ok(()) if complete => { + self.transmit.complete(); + self.tx_offset = 0; + } + Ok(()) | Err(UsbError::WouldBlock) => {} + Err(error) => panic!("CDC IN endpoint invariant: {error:?}"), + } + } +} diff --git a/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs b/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs index 8326cf6..2569575 100644 --- a/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs +++ b/firmware/platforms/stm32f7/tests/platform_safety_boundary.rs @@ -143,7 +143,7 @@ fn ordinary_f722_image_has_direct_configuration_after_output_inhibition() { for required in [ "config_file: F722ConfigFile", "file_service: FileService", - "receiver: IntegrityMavlinkReceiver<1>", + "uart_receiver: IntegrityMavlinkReceiver<1>", "file_lease_ticks: u16", "TRANSFER_REPLAY_LEASE_SECONDS", "let Some(message) = frame.decode()", diff --git a/firmware/platforms/stm32f7/tests/usb_mavlink.rs b/firmware/platforms/stm32f7/tests/usb_mavlink.rs new file mode 100644 index 0000000..5de68b4 --- /dev/null +++ b/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, + input: [Option>; 4], + output: [Option>; 4], + sizes: [[u16; 4]; 2], + stalled: [[bool; 4]; 2], + bulk_in: usize, + bulk_out: usize, +} + +#[derive(Clone, Default)] +struct Host(Arc>); + +struct PacketBus(Arc>); + +impl UsbBus for PacketBus { + fn alloc_ep( + &mut self, + direction: UsbDirection, + address: Option, + kind: EndpointType, + size: u16, + _: u8, + ) -> Result { + 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 { + 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 { + 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> { + 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> { + 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 { + 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::()); + 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::>(), + [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::>(), 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); + }); +}