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

//! 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]);
    }
}