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

//! MAVLink serial-byte receive API.

use crate::{MavlinkFrame, message};

/// Bounded MAVLink 2 receiver for a board's serial-byte interrupt.
///
/// Boards pass UART bytes to [`Self::receive_byte`] and take complete frames
/// from [`Self::take_received`]. Production has no signing verifier or replay
/// state, so it never exposes an inbound MAVLink frame as an authority record.
pub struct MavlinkReceiver<const N: usize> {
    frame: MavlinkFrame,
    received: usize,
    expected: usize,
    frames: [Option<MavlinkFrame>; N],
    front: usize,
    length: usize,
}

impl<const N: usize> Default for MavlinkReceiver<N> {
    fn default() -> Self {
        Self::new()
    }
}

impl<const N: usize> MavlinkReceiver<N> {
    /// Creates a production receiver with space for `N` complete frames.
    ///
    /// TODO(signing): accept a frame only after verified MAVLink-v2 signing,
    /// configured sender/target binding, persistent anti-replay state, and
    /// provisioning/rotation policy exist together. A V2 CRC only detects
    /// corruption; it is not authentication.
    pub const fn new() -> Self {
        Self {
            frame: MavlinkFrame::new(),
            received: 0,
            expected: 0,
            frames: [None; N],
            front: 0,
            length: 0,
        }
    }

    /// Receives one UART byte and returns whether it queued a complete frame.
    ///
    /// Bytes outside MAVLink 2 frames and frames with invalid CRCs are
    /// discarded. Signed frames are also discarded because this receiver has
    /// no verifier or replay state. Until the TODO(signing) work is complete,
    /// unsigned frames are deliberately not promoted to received records
    /// either.
    pub fn receive_byte(&mut self, byte: u8) -> bool {
        if self.received == 0 {
            if byte == 0xfd {
                self.frame.as_mut_slice()[0] = byte;
                self.received = 1;
            }
            return false;
        }

        self.frame.as_mut_slice()[self.received] = byte;
        self.received += 1;

        if self.received == 10 {
            let header = self.frame.as_slice();
            // MAVLink's bit 0 means the packet carries a signature. Reject all
            // nonzero incompatibility flags until a reviewed signature verifier
            // and persistent replay policy exist.
            if header[2] != 0 {
                self.reset();
                return false;
            }
            self.expected = 1 + 9 + usize::from(header[1]) + 2;
        }

        if self.expected != 0 && self.received == self.expected {
            let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
            self.received = 0;
            self.expected = 0;
            // The production receiver still drops every inbound frame until
            // signing/replay verification exists. Validate either known
            // dialect here so a future verifier receives no CRC-corrupt
            // custom configuration message.
            if !has_valid_known_crc(&frame) {
                return false;
            }
        }

        false
    }

    /// Discards the in-progress frame after a UART transport fault.
    pub fn reset(&mut self) {
        self.frame = MavlinkFrame::new();
        self.received = 0;
        self.expected = 0;
    }

    /// Takes the oldest complete received frame, if one is available.
    ///
    /// The production receiver returns no frame until signing verification is
    /// implemented, but retaining this API keeps dispatch ownership explicit.
    pub fn take_received(&mut self) -> Option<MavlinkFrame> {
        if self.length == 0 {
            return None;
        }
        let frame = self.frames[self.front].take();
        self.front = (self.front + 1) % N;
        self.length -= 1;
        frame
    }

    /// Returns whether at least one complete frame awaits dispatch.
    pub const fn has_received(&self) -> bool {
        self.length != 0
    }
}

fn has_valid_known_crc(frame: &MavlinkFrame) -> bool {
    let common_valid = frame.has_valid_crc::<message::MavMessage>();
    #[cfg(feature = "dialect-kaptan")]
    {
        common_valid || frame.has_valid_crc::<message::kaptan::MavMessage>()
    }
    #[cfg(not(feature = "dialect-kaptan"))]
    {
        common_valid
    }
}

#[cfg(test)]
mod tests {
    use super::MavlinkReceiver;
    use crate::{MavlinkFrame, message};

    fn heartbeat(sequence: u8) -> MavlinkFrame {
        let mut frame = MavlinkFrame::new();
        frame.serialize_message_data(
            message::MavHeader {
                sequence,
                system_id: 42,
                component_id: 7,
            },
            &message::HEARTBEAT_DATA {
                custom_mode: 0,
                mavtype: message::MavType::MAV_TYPE_QUADROTOR,
                autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
                base_mode: message::MavModeFlag::empty(),
                system_status: message::MavState::MAV_STATE_STANDBY,
                mavlink_version: 3,
            },
        );
        frame
    }

    fn heartbeat_v1(sequence: u8) -> mavlink::MAVLinkV1MessageRaw {
        let mut frame = mavlink::MAVLinkV1MessageRaw::new();
        frame.serialize_message_data(
            message::MavHeader {
                sequence,
                system_id: 42,
                component_id: 7,
            },
            &message::HEARTBEAT_DATA {
                custom_mode: 0,
                mavtype: message::MavType::MAV_TYPE_QUADROTOR,
                autopilot: message::MavAutopilot::MAV_AUTOPILOT_GENERIC,
                base_mode: message::MavModeFlag::empty(),
                system_status: message::MavState::MAV_STATE_STANDBY,
                mavlink_version: 3,
            },
        );
        frame
    }

    #[test]
    fn discards_a_crc_valid_mavlink_v1_frame() {
        let mut receiver = MavlinkReceiver::<1>::new();
        let v1 = heartbeat_v1(3);
        assert_eq!(v1.raw_bytes()[0], 0xfe);

        for byte in v1.raw_bytes() {
            assert!(!receiver.receive_byte(*byte));
        }

        assert_eq!(receiver.take_received(), None);
    }

    #[test]
    fn production_rejects_unsigned_frames_without_a_signing_verifier() {
        let frame = heartbeat(0);
        let mut receiver = MavlinkReceiver::<1>::new();
        for byte in frame.raw_bytes() {
            assert!(!receiver.receive_byte(*byte));
        }
        assert!(!receiver.has_received());
        assert_eq!(receiver.take_received(), None);
    }

    #[test]
    fn rejects_signed_v2_before_accepting_any_payload() {
        let mut receiver = MavlinkReceiver::<1>::new();
        // The first ten bytes contain the complete MAVLink 2 header. This
        // frame advertises a signature (incompatibility bit 0), so rejection
        // happens before its payload can be considered.
        for byte in [0xfd, 0, 1, 0, 0, 1, 1, 0, 0, 0] {
            assert!(!receiver.receive_byte(byte));
        }
        assert!(!receiver.has_received());
    }

    #[test]
    fn a_crc_error_cannot_create_a_received_record() {
        let frame = heartbeat(0);
        let mut bytes = frame.into_inner();
        bytes[10] ^= 1;
        let corrupt = MavlinkFrame::from_bytes_unparsed(bytes);
        let mut receiver = MavlinkReceiver::<1>::new();

        for byte in corrupt.raw_bytes() {
            assert!(!receiver.receive_byte(*byte));
        }
        assert!(!receiver.has_received());
    }
}