public/kaptan
1
Fork 0
Code Issues 24 Pull Requests Actions Packages Projects Releases Wiki Activity

feat(f722): add usb mavlink transport

Browse Source
This commit is contained in:
Onur Özkan committed 2026-09-06 23:14:00 +03:00
1 parent 7dc3ccb619
commit 604a7aba86
18 files changed
+1087 -208
Download Patch File Download Diff File Expand all files Collapse all files

No files matched your search

firmware/Cargo.lock
Generated
+72 -3
View File
@@ -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"
firmware/libs/kaptan-data-channel/Cargo.toml
+1 -1
View File
@@ -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 }
firmware/libs/kaptan-data-channel/src/lib.rs
+57 -108
View File
@@ -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<SERVICE_HZ>,
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<Self, DataChannelError> {
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<PendingFrame<MavlinkFrame>> {
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<M: MessageData<Message = MavMessage>>(
/// Encodes an adapter-owned generated MAVLink record through the channel's
/// sole sequence-number owner.
pub fn encode_message<M: MessageData<Message = MavMessage>>(
&mut self,
class: FrameClass,
message: &M,
) -> EnqueueResult {
self.sink.send(class, self.encoder.encode(message))
) -> PendingFrame<MavlinkFrame> {
PendingFrame {
class,
frame: self.encoder.encode(message),
}
}
fn refresh_vertical_data(&mut self) {
@@ -276,39 +247,15 @@ impl<const SERVICE_HZ: usize> Schedule<SERVICE_HZ> {
#[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::<MavMessage>());
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);
}
}
firmware/libs/kaptan-mavlink/src/receiver.rs
+22 -8
View File
@@ -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<IntegrityCheckedFrame> {
/// Consumes one byte, returning a complete integrity-checked frame if ready.
pub fn receive_byte(&mut self, byte: u8) -> Option<IntegrityCheckedFrame> {
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<const N: usize> {
framer: IntegrityFramer,
framer: IntegrityMavlinkFramer,
frames: [Option<IntegrityCheckedFrame>; N],
front: usize,
length: usize,
@@ -105,7 +119,7 @@ impl<const N: usize> IntegrityMavlinkReceiver<N> {
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,
firmware/libs/kaptan-tx/src/lib.rs
+14 -4
View File
@@ -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<Frame: Copy, const N: usize> TransmitQueue<Frame, N> {
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<PendingFrame<Frame>> {
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<PendingFrame<Frame>> {
self.active.take()
}
@@ -162,6 +162,14 @@ impl<Frame: Copy, const N: usize> TransmitQueue<Frame, N> {
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<Frame>> {
self.active.as_ref()
}
fn take_next(&mut self) -> Option<PendingFrame<Frame>> {
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]
firmware/platforms/stm32f7/Cargo.toml
+4
View File
@@ -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"
firmware/platforms/stm32f7/src/f722/board.rs
+86 -37
View File
@@ -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<C, I, const MAX_PAYLOAD_BYTES: usize>
where
C: Clone + Config,
I: F7ImmutableConfig,
{
data_channel: DataChannel<'static, F722MavlinkTransmitter, SERVICE_HZ>,
data_channel: DataChannel<'static, SERVICE_HZ>,
link: F722MavlinkLink,
config_file: F722ConfigFile<C, I, MAX_PAYLOAD_BYTES>,
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<C, I, const MAX_PAYLOAD_BYTES: usize>(
range: Option<F722Range>,
mtf01: Option<F722Mtf01>,
mtf01_uart: Option<Uart4>,
transmitter: F722MavlinkTransmitter,
link: F722MavlinkLink,
led: F722StatusLed,
configuration: F722ConfigManager<C, I, MAX_PAYLOAD_BYTES>,
) -> Result<F722InitResources<C, I, MAX_PAYLOAD_BYTES>, 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<super::peripherals::F722UsbMavlink, BoardError> {
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
firmware/platforms/stm32f7/src/f722/clock.rs
+2
View File
@@ -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)]
firmware/platforms/stm32f7/src/f722/data_channel.rs
+66 -28
View File
@@ -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<peripherals::DcacheDisabledDmaMemory, peripherals::Usart2TxDma, 4>),
}
/// 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<Self, ()> {
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<Self, ()> {
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<MavlinkFrame>) -> 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<MavlinkFrame>,
) -> bool {
match ingress {
MavlinkIngress::Uart => self.send_uart(pending),
MavlinkIngress::Usb => self.usb.send(pending),
}
}
fn send_uart(&mut self, pending: PendingFrame<MavlinkFrame>) -> 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<IntegrityCheckedFrame> {
self.usb.take_received()
}
}
firmware/platforms/stm32f7/src/f722/firmware.rs
+11
View File
@@ -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();
firmware/platforms/stm32f7/src/f722/mod.rs
+1
View File
@@ -15,6 +15,7 @@ mod motor_inhibit;
mod peripherals;
mod routes;
mod sensors;
mod usb;
use kaptan_config::Config;
firmware/platforms/stm32f7/src/f722/peripherals/clock.rs
+40 -17
View File
@@ -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;
}
firmware/platforms/stm32f7/src/f722/peripherals/gpio.rs
+17
View File
@@ -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()
firmware/platforms/stm32f7/src/f722/peripherals/mod.rs
+3 -1
View File
@@ -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};
firmware/platforms/stm32f7/src/f722/peripherals/usb.rs
+121
View File
@@ -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<F722UsbPeripheral>;
type F722UsbBusAllocator = UsbBusAllocator<F722UsbBus>;
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<F722UsbMavlink, ()> {
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))
}
firmware/platforms/stm32f7/src/f722/usb.rs
+135
View File
@@ -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<MavlinkFrame, 4>,
tx_offset: usize,
framer: IntegrityMavlinkFramer,
rx_packet: [u8; USB_PACKET_BYTES],
rx_remaining: Range<usize>,
}
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<MavlinkFrame>) -> 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<IntegrityCheckedFrame> {
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:?}"),
}
}
}
firmware/platforms/stm32f7/tests/platform_safety_boundary.rs
+1 -1
View File
@@ -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()",
firmware/platforms/stm32f7/tests/usb_mavlink.rs
+434
View File
@@ -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);
});
}