refactor: move firmware workspace under firmware

firmware/libs/kaptan-mavlink/src/transmitter.rs

@@ -0,0 +1,374 @@
+//! MAVLink frame encoding and UART transmission API.
+
+use kaptan_dma::{DmaTxBufferFactory, DmaTxEngine, TxCompletion};
+use kaptan_log::KaptanError;
+use kaptan_tx::{PendingFrame, TransmitQueue};
+
+use crate::MavlinkFrame;
+
+/// Delivery importance used for bounded MAVLink transmission admission.
+pub use kaptan_tx::{EnqueueResult, FrameClass};
+
+/// Failure while preparing or starting a MAVLink frame transfer.
+#[derive(Debug, KaptanError)]
+pub enum MavlinkTransmitFault<MemoryError, EngineError> {
+    /// DMA-visible frame preparation failed.
+    Memory(MemoryError),
+    /// The UART DMA route rejected a transfer start.
+    Engine(EngineError),
+}
+
+/// Transport-neutral class of a terminal link fault.
+#[derive(Clone, Copy, Debug, Eq, PartialEq)]
+pub enum TransmitFaultKind {
+    /// DMA-visible frame preparation failed.
+    Memory,
+    /// The UART DMA engine rejected a transfer start.
+    Engine,
+    /// An active DMA transfer ended with a terminal fault or halt.
+    Terminal,
+}
+
+impl<MemoryError, EngineError> MavlinkTransmitFault<MemoryError, EngineError> {
+    /// Returns the bounded category a platform may convert to a static log
+    /// message without exposing a generic HAL error value on the wire.
+    pub const fn kind(&self) -> TransmitFaultKind {
+        match self {
+            Self::Memory(_) => TransmitFaultKind::Memory,
+            Self::Engine(_) => TransmitFaultKind::Engine,
+        }
+    }
+}
+
+/// Combined bounded-admission and DMA-start outcome.
+///
+/// The two outcomes stay distinct so a platform can account for a failed DMA
+/// start without falsely treating a queue-full source frame as retained.
+#[must_use]
+pub struct AdmissionStart<MemoryError, EngineError> {
+    /// Queue admission for the source frame.
+    pub admission: EnqueueResult,
+    /// One bounded attempt to start pending DMA work.
+    pub start: Result<(), MavlinkTransmitFault<MemoryError, EngineError>>,
+}
+
+/// Owns MAVLink frame admission and UART DMA lifecycle.
+///
+/// Exactly one worker invokes this type. It accepts local encoded frames and
+/// received frames alike; the caller owns the policy that decides which frames
+/// are transmitted.
+pub struct MavlinkTransmitter<Memory, Engine, const N: usize> {
+    transmit: TransmitQueue<MavlinkFrame, N>,
+    memory: Memory,
+    engine: Engine,
+}
+
+impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N> {
+    /// Creates an empty MAVLink transmitter.
+    pub const fn new(memory: Memory, engine: Engine) -> Self {
+        Self {
+            transmit: TransmitQueue::new(),
+            memory,
+            engine,
+        }
+    }
+}
+
+impl<Memory, Engine, const N: usize> MavlinkTransmitter<Memory, Engine, N>
+where
+    Memory: DmaTxBufferFactory,
+    Engine: DmaTxEngine,
+{
+    /// Admits an already-serialized MAVLink frame for UART transmission.
+    ///
+    /// A full queue is a normal bounded-admission outcome, returned as
+    /// [`EnqueueResult`]. DMA preparation and transfer-start failures remain
+    /// [`MavlinkTransmitFault`] errors.
+    pub fn send_frame(
+        &mut self,
+        class: FrameClass,
+        frame: MavlinkFrame,
+    ) -> Result<EnqueueResult, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
+        let outcome = self.admit_and_start(class, frame);
+        outcome.start.map(|()| outcome.admission)
+    }
+
+    /// Admits one frame and attempts one bounded DMA start while preserving
+    /// both outcomes. Platform policy uses this to account for a failed DMA
+    /// start without mistaking an admission rejection for a retained frame.
+    /// A start failure retains the selected frame in the queue reservation for
+    /// a later service-turn retry.
+    pub fn admit_and_start(
+        &mut self,
+        class: FrameClass,
+        frame: MavlinkFrame,
+    ) -> AdmissionStart<Memory::Error, Engine::StartError> {
+        let admission = self.enqueue_frame(class, frame);
+        let start = self.start_pending();
+        AdmissionStart { admission, start }
+    }
+
+    /// Admits a frame without attempting to start the DMA engine.
+    ///
+    /// This split lifecycle is for hardware routes whose terminal DMA IRQ can
+    /// arrive a few bus cycles before the stream's `EN` bit reads clear. Such
+    /// a route may acknowledge a completion, retain the selected frame, and
+    /// retry [`Self::start_pending`] from its next bounded service turn rather
+    /// than treating that transient `Busy` observation as a transport fault.
+    /// Callers still own the policy for a queue-full result.
+    pub fn enqueue_frame(&mut self, class: FrameClass, frame: MavlinkFrame) -> EnqueueResult {
+        self.transmit.enqueue(PendingFrame { class, frame })
+    }
+
+    /// Attempts to start one pending DMA frame.
+    ///
+    /// A rejected engine start leaves the exact frame reserved for a later
+    /// retry. This method therefore does not discard or overwrite DMA source
+    /// storage on a transient hardware-not-ready result.
+    pub fn start_pending(
+        &mut self,
+    ) -> Result<(), MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
+        self.start_next()
+    }
+
+    /// Releases a terminal DMA frame and opportunistically starts the next.
+    ///
+    /// The returned class is the frame that reached a terminal DMA result,
+    /// not a class selected from newly pending work. A policy can therefore
+    /// distinguish completion classes without inferring them from newly
+    /// pending work.
+    pub fn on_dma_complete(
+        &mut self,
+        completion: TxCompletion,
+    ) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
+        let completed = self.complete_dma(completion)?;
+        if completion == TxCompletion::Complete {
+            self.start_pending()?;
+        }
+        Ok(completed)
+    }
+
+    /// Releases the active frame after a terminal DMA result without starting
+    /// another one.
+    ///
+    /// Most routes use [`Self::on_dma_complete`], which preserves legacy
+    /// immediate restart behavior. A route whose hardware exposes terminal
+    /// status before its enable bit is observably low uses this method and
+    /// calls [`Self::start_pending`] from a later bounded service turn.
+    pub fn complete_dma(
+        &mut self,
+        completion: TxCompletion,
+    ) -> Result<Option<FrameClass>, MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
+        match completion {
+            TxCompletion::NotPending => Ok(None),
+            TxCompletion::Complete => Ok(self.transmit.complete().map(|frame| frame.class)),
+            TxCompletion::TransferFault | TxCompletion::Halted => {
+                Ok(self.transmit.fail().map(|frame| frame.class))
+            }
+        }
+    }
+
+    fn start_next(
+        &mut self,
+    ) -> Result<(), MavlinkTransmitFault<Memory::Error, Engine::StartError>> {
+        let (transmit, memory, engine) = (&mut self.transmit, &mut self.memory, &mut self.engine);
+        transmit
+            .start_with(|frame| {
+                let buffer = memory
+                    .prepare(frame.raw_bytes())
+                    .map_err(MavlinkTransmitFault::Memory)?;
+                engine.start(buffer).map_err(MavlinkTransmitFault::Engine)
+            })
+            .map(|_| ())
+    }
+}
+
+#[cfg(test)]
+mod tests {
+    use super::{EnqueueResult, FrameClass, MavlinkTransmitFault, MavlinkTransmitter};
+    use crate::{encoder::MavlinkEncoder, message::MavState};
+    use kaptan_dma::{DmaReadableBuffer, DmaTxBufferFactory, DmaTxEngine, TxCompletion};
+
+    struct Memory;
+
+    impl DmaTxBufferFactory for Memory {
+        type Error = core::convert::Infallible;
+
+        fn prepare<'a>(&mut self, bytes: &'a [u8]) -> Result<DmaReadableBuffer<'a>, Self::Error> {
+            // SAFETY: this host-only test keeps the queue's active frame alive.
+            Ok(unsafe { DmaReadableBuffer::new_unchecked(bytes) })
+        }
+    }
+
+    struct Engine;
+
+    impl DmaTxEngine for Engine {
+        type StartError = core::convert::Infallible;
+
+        fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
+            Ok(())
+        }
+    }
+
+    struct RecordingEngine {
+        started_sequences: [u8; 4],
+        starts: usize,
+    }
+
+    #[derive(Clone, Copy, Debug, Eq, PartialEq)]
+    enum RetryError {
+        Busy,
+    }
+
+    struct BusyOnceEngine {
+        busy_once: bool,
+        starts: usize,
+    }
+
+    impl DmaTxEngine for BusyOnceEngine {
+        type StartError = RetryError;
+
+        fn start(&mut self, _: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
+            if self.busy_once {
+                self.busy_once = false;
+                return Err(RetryError::Busy);
+            }
+            self.starts += 1;
+            Ok(())
+        }
+    }
+
+    impl DmaTxEngine for RecordingEngine {
+        type StartError = core::convert::Infallible;
+
+        fn start(&mut self, buffer: DmaReadableBuffer<'_>) -> Result<(), Self::StartError> {
+            self.started_sequences[self.starts] = buffer.bytes()[4];
+            self.starts += 1;
+            Ok(())
+        }
+    }
+
+    #[test]
+    fn transfer_fault_drops_only_the_active_frame() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let mut transmitter = MavlinkTransmitter::<Memory, Engine, 2>::new(Memory, Engine);
+        transmitter
+            .send_frame(
+                FrameClass::BestEffort,
+                encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+            )
+            .unwrap();
+        transmitter
+            .send_frame(
+                FrameClass::BestEffort,
+                encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+            )
+            .unwrap();
+        assert!(transmitter.transmit.is_active());
+
+        transmitter
+            .on_dma_complete(TxCompletion::TransferFault)
+            .unwrap();
+        assert!(!transmitter.transmit.is_active());
+    }
+
+    #[test]
+    fn reports_critical_queue_saturation_to_the_policy_owner() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let mut transmitter = MavlinkTransmitter::<Memory, Engine, 1>::new(Memory, Engine);
+
+        assert_eq!(
+            transmitter
+                .send_frame(
+                    FrameClass::Critical,
+                    encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+                )
+                .unwrap(),
+            EnqueueResult::Enqueued
+        );
+        assert_eq!(
+            transmitter
+                .send_frame(
+                    FrameClass::Critical,
+                    encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+                )
+                .unwrap(),
+            EnqueueResult::Enqueued
+        );
+        assert_eq!(
+            transmitter
+                .send_frame(
+                    FrameClass::Critical,
+                    encoder.heartbeat(MavState::MAV_STATE_STANDBY, false),
+                )
+                .unwrap(),
+            EnqueueResult::CriticalQueueFull
+        );
+    }
+
+    #[test]
+    fn terminal_completion_can_defer_a_transient_engine_busy_without_losing_frame_order() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let mut transmitter = MavlinkTransmitter::<Memory, BusyOnceEngine, 2>::new(
+            Memory,
+            BusyOnceEngine {
+                busy_once: false,
+                starts: 0,
+            },
+        );
+        let state = MavState::MAV_STATE_STANDBY;
+
+        transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
+        transmitter.start_pending().unwrap();
+        assert_eq!(transmitter.engine.starts, 1);
+        transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
+
+        // Model the STM32 case: TCIF is acknowledged but CR.EN still reports
+        // busy for the first attempt to start the successor. `complete_dma`
+        // deliberately does not start it in the terminal IRQ path.
+        assert_eq!(
+            transmitter.complete_dma(TxCompletion::Complete).unwrap(),
+            Some(FrameClass::Critical)
+        );
+        assert_eq!(transmitter.engine.starts, 1);
+        transmitter.engine.busy_once = true;
+        assert!(matches!(
+            transmitter.start_pending(),
+            Err(MavlinkTransmitFault::Engine(RetryError::Busy))
+        ));
+        assert!(!transmitter.transmit.is_active());
+
+        transmitter.start_pending().unwrap();
+        assert!(transmitter.transmit.is_active());
+        assert_eq!(transmitter.engine.starts, 2);
+    }
+
+    #[test]
+    fn completion_retry_starts_pending_best_effort_before_later_critical_frame() {
+        let mut encoder = MavlinkEncoder::new(1, 1);
+        let mut transmitter = MavlinkTransmitter::<Memory, RecordingEngine, 3>::new(
+            Memory,
+            RecordingEngine {
+                started_sequences: [0; 4],
+                starts: 0,
+            },
+        );
+        let state = MavState::MAV_STATE_STANDBY;
+
+        // A critical frame is active. A best-effort sample arrives behind it
+        // before DMA completion.
+        transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
+        transmitter.start_pending().unwrap();
+        transmitter.enqueue_frame(FrameClass::BestEffort, encoder.heartbeat(state, false));
+
+        // The completion worker performs its bounded retry before the next
+        // periodic sender can enqueue fresh critical traffic. The existing
+        // best-effort frame therefore cannot starve behind later traffic.
+        transmitter.complete_dma(TxCompletion::Complete).unwrap();
+        transmitter.start_pending().unwrap();
+        transmitter.enqueue_frame(FrameClass::Critical, encoder.heartbeat(state, false));
+
+        assert_eq!(transmitter.engine.starts, 2);
+        assert_eq!(&transmitter.engine.started_sequences[..2], &[0, 1]);
+    }
+}