feat(mavlink): adopt the kaptan-native dialect

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

@@ -1,6 +1,36 @@
 //! MAVLink serial-byte receive API.
 
-use crate::{MavlinkFrame, message};
+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.
+#[repr(transparent)]
+pub struct IntegrityCheckedFrame(MavlinkFrame);
+
+const _: [(); core::mem::size_of::<MavlinkFrame>()] =
+    [(); core::mem::size_of::<IntegrityCheckedFrame>()];
+const _: [(); core::mem::align_of::<MavlinkFrame>()] =
+    [(); core::mem::align_of::<IntegrityCheckedFrame>()];
+
+impl IntegrityCheckedFrame {
+    fn try_new(frame: MavlinkFrame) -> Option<Self> {
+        let message_id = frame.message_id();
+        (MavMessage::all_ids().contains(&message_id) && frame.has_valid_crc::<MavMessage>())
+            .then_some(Self(frame))
+    }
+
+    /// Returns the admitted raw frame without copying it.
+    pub const fn raw(&self) -> &MavlinkFrame {
+        &self.0
+    }
+
+    /// Decodes the admitted Kaptan payload without repeating its CRC check.
+    pub fn decode(&self) -> Option<MavMessage> {
+        decode_message(&self.0)
+    }
+}
 
 struct IntegrityFramer {
     frame: MavlinkFrame,
@@ -17,7 +47,7 @@ impl IntegrityFramer {
         }
     }
 
-    fn receive_byte(&mut self, byte: u8) -> Option<MavlinkFrame> {
+    fn receive_byte(&mut self, byte: u8) -> Option<IntegrityCheckedFrame> {
         if self.received == 0 {
             if byte == 0xfd {
                 self.frame.as_mut_slice()[0] = byte;
@@ -48,7 +78,7 @@ impl IntegrityFramer {
         let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
         self.received = 0;
         self.expected = 0;
-        has_valid_known_crc(&frame).then_some(frame)
+        IntegrityCheckedFrame::try_new(frame)
     }
 
     fn reset(&mut self) {
@@ -62,10 +92,10 @@ impl IntegrityFramer {
 ///
 /// 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.
+/// and how a received request may be admitted.
 pub struct IntegrityMavlinkReceiver<const N: usize> {
     framer: IntegrityFramer,
-    frames: [Option<MavlinkFrame>; N],
+    frames: [Option<IntegrityCheckedFrame>; N],
     front: usize,
     length: usize,
 }
@@ -76,7 +106,7 @@ impl<const N: usize> IntegrityMavlinkReceiver<N> {
         assert!(N != 0, "integrity receiver capacity must be nonzero");
         Self {
             framer: IntegrityFramer::new(),
-            frames: [None; N],
+            frames: [const { None }; N],
             front: 0,
             length: 0,
         }
@@ -99,13 +129,13 @@ impl<const N: usize> IntegrityMavlinkReceiver<N> {
     /// Discards partial framing state and all queued unauthenticated frames.
     pub fn reset(&mut self) {
         self.framer.reset();
-        self.frames = [None; N];
+        self.frames = [const { None }; N];
         self.front = 0;
         self.length = 0;
     }
 
     /// Takes the oldest integrity-checked frame.
-    pub fn take_received(&mut self) -> Option<MavlinkFrame> {
+    pub fn take_received(&mut self) -> Option<IntegrityCheckedFrame> {
         if self.length == 0 {
             return None;
         }
@@ -122,59 +152,107 @@ impl<const N: usize> Default for IntegrityMavlinkReceiver<N> {
     }
 }
 
-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};
+    use super::{IntegrityCheckedFrame, IntegrityMavlinkReceiver};
+    use crate::{
+        MavlinkFrame,
+        generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage},
+    };
+    use mavlink_core::{
+        MAVLinkV1MessageRaw, MavHeader, MavlinkVersion, MessageData, error::ParserError,
+    };
+
+    struct ForeignHeartbeat;
+
+    impl MessageData for ForeignHeartbeat {
+        type Message = MavMessage;
+
+        const ID: u32 = 0;
+        const NAME: &'static str = "FOREIGN_HEARTBEAT";
+        const EXTRA_CRC: u8 = 50;
+        const ENCODED_LEN: usize = 9;
+
+        fn ser(&self, _: MavlinkVersion, payload: &mut [u8]) -> usize {
+            payload[..Self::ENCODED_LEN].copy_from_slice(&[0, 0, 0, 0, 0, 0, 0, 0, 3]);
+            Self::ENCODED_LEN
+        }
+
+        fn deser(_: MavlinkVersion, _: &[u8]) -> Result<Self, ParserError> {
+            Ok(Self)
+        }
+    }
+
+    struct MalformedKaptanHeartbeat;
+
+    impl MessageData for MalformedKaptanHeartbeat {
+        type Message = MavMessage;
+
+        const ID: u32 = HEARTBEAT_DATA::ID;
+        const NAME: &'static str = HEARTBEAT_DATA::NAME;
+        const EXTRA_CRC: u8 = HEARTBEAT_DATA::EXTRA_CRC;
+        const ENCODED_LEN: usize = HEARTBEAT_DATA::ENCODED_LEN;
+
+        fn ser(&self, _: MavlinkVersion, payload: &mut [u8]) -> usize {
+            payload[0] = 0;
+            Self::ENCODED_LEN
+        }
+
+        fn deser(_: MavlinkVersion, _: &[u8]) -> Result<Self, ParserError> {
+            Ok(Self)
+        }
+    }
 
     fn heartbeat(sequence: u8) -> MavlinkFrame {
         let mut frame = MavlinkFrame::new();
         frame.serialize_message_data(
-            message::MavHeader {
+            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,
+            &HEARTBEAT_DATA {
+                state: HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED,
             },
         );
         frame
     }
 
-    fn heartbeat_v1(sequence: u8) -> mavlink::MAVLinkV1MessageRaw {
-        let mut frame = mavlink::MAVLinkV1MessageRaw::new();
+    fn foreign_heartbeat_v1(sequence: u8) -> MAVLinkV1MessageRaw {
+        let mut frame = MAVLinkV1MessageRaw::new();
         frame.serialize_message_data(
-            message::MavHeader {
+            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,
+            &ForeignHeartbeat,
+        );
+        frame
+    }
+
+    fn foreign_heartbeat_v2(sequence: u8) -> MavlinkFrame {
+        let mut frame = MavlinkFrame::new();
+        frame.serialize_message_data(
+            MavHeader {
+                sequence,
+                system_id: 42,
+                component_id: 7,
             },
+            &ForeignHeartbeat,
+        );
+        frame
+    }
+
+    fn malformed_kaptan_heartbeat(sequence: u8) -> MavlinkFrame {
+        let mut frame = MavlinkFrame::new();
+        frame.serialize_message_data(
+            MavHeader {
+                sequence,
+                system_id: 42,
+                component_id: 7,
+            },
+            &MalformedKaptanHeartbeat,
         );
         frame
     }
@@ -182,14 +260,26 @@ mod tests {
     #[test]
     fn discards_a_crc_valid_mavlink_v1_frame() {
         let mut receiver = IntegrityMavlinkReceiver::<1>::new();
-        let v1 = heartbeat_v1(3);
+        let v1 = foreign_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);
+        assert!(receiver.take_received().is_none());
+    }
+
+    #[test]
+    fn discards_a_crc_valid_foreign_dialect_frame() {
+        let foreign = foreign_heartbeat_v2(3);
+        let mut receiver = IntegrityMavlinkReceiver::<1>::new();
+
+        for byte in foreign.raw_bytes() {
+            assert!(!receiver.receive_byte(*byte));
+        }
+
+        assert!(receiver.take_received().is_none());
     }
 
     #[test]
@@ -199,8 +289,35 @@ mod tests {
         for byte in frame.raw_bytes() {
             receiver.receive_byte(*byte);
         }
-        assert_eq!(receiver.take_received(), Some(frame));
-        assert_eq!(receiver.take_received(), None);
+        let received = receiver.take_received().expect("complete frame");
+        assert_eq!(received.raw(), &frame);
+        assert_eq!(received.decode(), crate::decode_message(&frame));
+        assert!(receiver.take_received().is_none());
+    }
+
+    #[test]
+    fn defers_payload_decoding_until_after_integrity_admission() {
+        let frame = malformed_kaptan_heartbeat(9);
+        let mut receiver = IntegrityMavlinkReceiver::<1>::new();
+        for byte in frame.raw_bytes() {
+            receiver.receive_byte(*byte);
+        }
+
+        let received = receiver.take_received().expect("CRC-valid Kaptan frame");
+        assert_eq!(received.raw(), &frame);
+        assert_eq!(received.decode(), None);
+    }
+
+    #[test]
+    fn integrity_wrapper_has_exactly_the_raw_frame_layout() {
+        assert_eq!(
+            core::mem::size_of::<IntegrityCheckedFrame>(),
+            core::mem::size_of::<MavlinkFrame>()
+        );
+        assert_eq!(
+            core::mem::align_of::<IntegrityCheckedFrame>(),
+            core::mem::align_of::<MavlinkFrame>()
+        );
     }
 
     #[test]
@@ -212,7 +329,7 @@ mod tests {
         for byte in [0xfd, 0, 1, 0, 0, 1, 1, 0, 0, 0] {
             assert!(!receiver.receive_byte(byte));
         }
-        assert_eq!(receiver.take_received(), None);
+        assert!(receiver.take_received().is_none());
     }
 
     #[test]
@@ -226,6 +343,6 @@ mod tests {
         for byte in corrupt.raw_bytes() {
             assert!(!receiver.receive_byte(*byte));
         }
-        assert_eq!(receiver.take_received(), None);
+        assert!(receiver.take_received().is_none());
     }
 }