src: decoder: Fix: Discard higher invalid msg_id

bluerobotics · Apr 2, 2024 · 5080b45 · 5080b45
1 parent 5deb5c3
commit 5080b45
Showing 1 changed file with 10 additions and 1 deletion.
diff --git a/src/decoder.rs b/src/decoder.rs
@@ -3,6 +3,7 @@ use crate::message::{ProtocolMessage, HEADER};
 #[derive(Debug)]
 pub enum ParseError {
     InvalidStartByte,
+    InvalidMessageId,
     IncompleteData,
     ChecksumError,
 }
@@ -62,7 +63,15 @@ impl Decoder {
                 if self.buffer.len() == 6 {
                     self.message.payload_length =
                         u16::from_le_bytes([self.buffer[0], self.buffer[1]]);
-                    self.message.message_id = u16::from_le_bytes([self.buffer[2], self.buffer[3]]);
+
+                    let msg_id = u16::from_le_bytes([self.buffer[2], self.buffer[3]]);
+                    const INVALID_MSG_MASK: u16 = 0b1111000000000000;
+                    if msg_id & INVALID_MSG_MASK != 0 {
+                        self.reset();
+                        return DecoderResult::Error(ParseError::InvalidMessageId)
+                    }
+                    self.message.message_id = msg_id;
+
                     self.message.src_device_id = self.buffer[4];
                     self.message.dst_device_id = self.buffer[5];
                     self.state = DecoderState::ReadingPayload;