add crc checking

Cargo.toml

@@ -22,6 +22,7 @@ cargo = "warn"
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
 [dependencies]
-num_enum = { version = "=0.5", default-features = false }
+num_enum = { version = ">=0.5", default-features = false }
-bitfield = { version = "=0.13" }
+bitfield = ">=0.13"
-heapless = { version = "=0.7" }
+heapless = ">=0.7"
+crc = ">=3.0"

src/lib.rs

@@ -1,8 +1,9 @@
 #![cfg_attr(not(test), no_std)]
 
 use bitfield::bitfield;
-use heapless::HistoryBuffer;
+use crc::{Crc, CRC_8_DVB_S2};
-use num_enum::{IntoPrimitive, TryFromPrimitive, TryFromPrimitiveError};
+use heapless::Deque;
+use num_enum::{IntoPrimitive, TryFromPrimitive};
 
 #[derive(Debug, PartialEq, Eq, Clone, Copy, IntoPrimitive, TryFromPrimitive)]
 #[repr(u8)]
@@ -56,28 +57,14 @@ pub enum Address {
 }
 
 #[derive(Debug, PartialEq, Eq, Clone, Copy)]
-struct Header {
+pub struct Header {
     address: Address,
     size: u8,
     frame_type: FrameType,
 }
 
-impl Header {
-    fn try_from_bytes(bytes: [u8; 3]) -> Result<Self, u8> {
-        Ok(Self {
-            address: bytes[0]
-                .try_into()
-                .map_err(|TryFromPrimitiveError { number }| number)?,
-            size: bytes[1],
-            frame_type: bytes[2]
-                .try_into()
-                .map_err(|TryFromPrimitiveError { number }| number)?,
-        })
-    }
-}
-
 bitfield! {
-    struct RcChannels([u8]);
+    struct RcChannelsPacked([u8]);
     impl Debug;
     u16;
     ch1, set_ch1: Self::upper(1), Self::lower(1);
@@ -98,7 +85,7 @@ bitfield! {
     ch16, set_ch16: Self::upper(16), Self::lower(16);
 }
 
-impl<T> RcChannels<T> {
+impl<T> RcChannelsPacked<T> {
     const BITS_PER_CHANNEL: usize = 11;
 
     const fn upper(ch: usize) -> usize {
@@ -111,21 +98,107 @@ impl<T> RcChannels<T> {
 }
 
 pub struct CrsfParser {
-    buffer: HistoryBuffer<u8, { Self::MAX_PACKET_SIZE }>,
+    buffer: Deque<u8, { Self::MAX_PACKET_SIZE }>,
 }
 
 impl CrsfParser {
     const MAX_PACKET_SIZE: usize = 64;
+
+    #[must_use]
+    pub const fn new() -> Self {
+        let buffer = Deque::new();
+        Self { buffer }
+    }
+
+    pub fn push(&mut self, byte: u8) {
+        if self.buffer.is_full() {
+            // deque used as ringbuffer. The oldest data is overwritten.
+            self.buffer.pop_front();
+        }
+        let _ = self.buffer.push_front(byte);
+    }
+
+    pub fn next_packet(&mut self) -> Option<Header> {
+        const CRC8_ALG: Crc<u8> = Crc::<u8>::new(&CRC_8_DVB_S2);
+
+        while Address::try_from(*self.buffer.back()?).is_err() {
+            // The buffer doesn't start with a valid Address
+            self.buffer.pop_back()?;
+        }
+        if self.buffer.len() < 4 {
+            // The buffer doesn't even have enough data to contain a valid header + crc byte
+            return None;
+        }
+
+        // Parse the packet header
+        let mut iter = self.buffer.iter().rev().copied();
+        let address = Address::try_from(iter.next()?).ok()?;
+        let size = iter.next()?;
+        let frame_type = FrameType::try_from(iter.next()?).ok()?;
+        let header = Header {
+            address,
+            size,
+            frame_type,
+        };
+        // TODO: check the frame size against the frame type.
+
+        // The size field contains the number of bytes coming after it.
+        let packet_length = header.size + 2;
+        if self.buffer.len() < usize::from(packet_length) {
+            // The buffer doesn't have enough data to contain the specified packet length
+            return None;
+        }
+
+        let mut crc_calc = CRC8_ALG.digest();
+        crc_calc.update(&[header.frame_type.into()]);
+        let payload_iter = iter.clone().take(usize::from(packet_length - 4));
+        for _ in 0..packet_length - 4 {
+            crc_calc.update(&[iter.next()?]);
+        }
+        let crc8 = crc_calc.finalize();
+        let crc = iter.next()?;
+        if crc != crc8 {
+            // Invalid checksum. Make sure to pop The oldest byte so we don't parse this packet
+            // again
+            self.buffer.pop_back()?;
+            return None;
+        }
+
+        // TODO: decode payload using type field
+
+        // sucessfully decoded a packet
+        for _ in 0..packet_length {
+            self.buffer.pop_back()?;
+        }
+        Some(header)
+    }
 }
 
 #[cfg(test)]
 mod tests {
+    use crate::{Address, CrsfParser, FrameType, Header};
+
     #[test]
     fn rc_channels() {
         let input = [
-            0xc8, 0x18, 0x16, 0xe0, 0x3, 0x1f, 0xb3, 0xc0, 0xf7, 0xb, 0xee, 0x69, 0x8f, 0x15, 0xe0,
+            0xc8, 0x18, 0x16, 0xe0, 0x03, 0x1f, 0xb3, 0xc0, 0xf7, 0x0b, 0xee, 0x69, 0x8f, 0x15,
-            0x3, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x78, 0xd9, 0xdb, 0xb4, 0xc8,
+            0xe0, 0x03, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x78, 0xd9, 0xdb, 0xb4,
         ];
-        unimplemented!();
+
+        let mut parser = CrsfParser::new();
+        for byte in input {
+            assert_eq!(parser.next_packet(), None);
+            parser.push(byte);
+        }
+
+        assert_eq!(
+            parser.next_packet(),
+            Some(Header {
+                address: Address::FlightController,
+                size: 24,
+                frame_type: FrameType::RcChannelsPacked
+            })
+        );
+        assert_eq!(parser.next_packet(), None);
     }
 }