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

//! MAVLink serial-byte receive API.

use crate::{MavlinkFrame, message};

struct IntegrityFramer {
    frame: MavlinkFrame,
    received: usize,
    expected: usize,
}

impl IntegrityFramer {
    const fn new() -> Self {
        Self {
            frame: MavlinkFrame::new(),
            received: 0,
            expected: 0,
        }
    }

    fn receive_byte(&mut self, byte: u8) -> Option<MavlinkFrame> {
        if self.received == 0 {
            if byte == 0xfd {
                self.frame.as_mut_slice()[0] = byte;
                self.received = 1;
            }
            return None;
        }

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

        if self.received == 10 {
            let header = self.frame.as_slice();
            // MAVLink incompatibility bit zero adds a signature. This framer
            // has neither a signing verifier nor persistent replay state, so
            // every nonzero incompatibility flag is rejected at the header.
            if header[2] != 0 {
                self.reset();
                return None;
            }
            self.expected = 1 + 9 + usize::from(header[1]) + 2;
        }

        if self.expected == 0 || self.received != self.expected {
            return None;
        }

        let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
        self.received = 0;
        self.expected = 0;
        has_valid_known_crc(&frame).then_some(frame)
    }

    fn reset(&mut self) {
        self.frame = MavlinkFrame::new();
        self.received = 0;
        self.expected = 0;
    }
}

/// 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 command may be admitted.
pub struct IntegrityMavlinkReceiver<const N: usize> {
    framer: IntegrityFramer,
    frames: [Option<MavlinkFrame>; N],
    front: usize,
    length: usize,
}

impl<const N: usize> IntegrityMavlinkReceiver<N> {
    /// Creates an empty integrity-only receiver.
    pub const fn new() -> Self {
        assert!(N != 0, "integrity receiver capacity must be nonzero");
        Self {
            framer: IntegrityFramer::new(),
            frames: [None; N],
            front: 0,
            length: 0,
        }
    }

    /// Queues one complete CRC-valid unsigned frame when capacity remains.
    pub fn receive_byte(&mut self, byte: u8) -> bool {
        let Some(frame) = self.framer.receive_byte(byte) else {
            return false;
        };
        if self.length == N {
            return false;
        }
        let index = (self.front + self.length) % N;
        self.frames[index] = Some(frame);
        self.length += 1;
        true
    }

    /// Discards partial framing state and all queued unauthenticated frames.
    pub fn reset(&mut self) {
        self.framer.reset();
        self.frames = [None; N];
        self.front = 0;
        self.length = 0;
    }

    /// Takes the oldest integrity-checked frame.
    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
    }
}

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

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::IntegrityMavlinkReceiver;
    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 = IntegrityMavlinkReceiver::<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 queues_a_complete_crc_valid_unsigned_frame() {
        let frame = heartbeat(9);
        let mut receiver = IntegrityMavlinkReceiver::<1>::new();
        for byte in frame.raw_bytes() {
            receiver.receive_byte(*byte);
        }
        assert_eq!(receiver.take_received(), Some(frame));
        assert_eq!(receiver.take_received(), None);
    }

    #[test]
    fn rejects_signed_v2_before_accepting_any_payload() {
        let mut receiver = IntegrityMavlinkReceiver::<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_eq!(receiver.take_received(), None);
    }

    #[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 = IntegrityMavlinkReceiver::<1>::new();

        for byte in corrupt.raw_bytes() {
            assert!(!receiver.receive_byte(*byte));
        }
        assert_eq!(receiver.take_received(), None);
    }
}