discard rejected mavlink frames entirely

mavlink-core/src/lib.rs

@@ -794,14 +794,15 @@ fn try_decode_v1<M: Message, R: Read>(
         .mut_payload_and_checksum()
         .copy_from_slice(payload_and_checksum);
 
-    // retry if CRC failed after previous STX
-    // (an STX byte may appear in the middle of a message)
-    if message.has_valid_crc::<M>() {
-        reader.consume(message.raw_bytes().len());
-        Ok(Some(message))
-    } else {
-        Ok(None)
+    // Skip a rejected MAVLink 1 frame as one frame. MAV_STX bytes inside its
+    // payload must not be treated as new packets.
+    let has_valid_crc = message.has_valid_crc::<M>();
+    reader.consume(packet_length);
+    if !has_valid_crc {
+        return Ok(None);
     }
+
+    Ok(Some(message))
 }
 
 #[cfg(feature = "tokio")]
@@ -822,14 +823,15 @@ async fn try_decode_v1_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
         .mut_payload_and_checksum()
         .copy_from_slice(payload_and_checksum);
 
-    // retry if CRC failed after previous STX
-    // (an STX byte may appear in the middle of a message)
-    if message.has_valid_crc::<M>() {
-        reader.consume(message.raw_bytes().len() - 1);
-        Ok(Some(message))
-    } else {
-        Ok(None)
+    // MAV_STX has already been consumed. Skip a rejected frame as one frame,
+    // because marker bytes inside its payload must not be treated as new packets.
+    let has_valid_crc = message.has_valid_crc::<M>();
+    reader.consume(packet_length);
+    if !has_valid_crc {
+        return Ok(None);
     }
+
+    Ok(Some(message))
 }
 
 /// Read a raw MAVLink 1 message from a [`PeekReader`].
@@ -849,8 +851,6 @@ pub fn read_v1_raw_message<M: Message, R: Read>(
         if let Some(msg) = try_decode_v1::<M, _>(reader)? {
             return Ok(msg);
         }
-
-        reader.consume(1);
     }
 }
 
@@ -1347,24 +1347,25 @@ fn try_decode_v2<M: Message, R: Read>(
     let header = &reader.peek_exact(whole_header_size)?[1..whole_header_size];
     message.mut_header().copy_from_slice(header);
 
+    // Skip a rejected MAVLink 2 frame as one frame. MAV_STX_V2 bytes inside
+    // its payload must not be treated as new packets.
+    let packet_length = message.raw_bytes().len();
     if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
-        // if there are incompatibility flags set that we do not know discard the message
-        reader.consume(1);
+        reader.peek_exact(packet_length)?;
+        reader.consume(packet_length);
         return Ok(None);
     }
 
-    let packet_length = message.raw_bytes().len();
     let payload_and_checksum_and_sign =
         &reader.peek_exact(packet_length)?[whole_header_size..packet_length];
     message
         .mut_payload_and_checksum_and_sign()
         .copy_from_slice(payload_and_checksum_and_sign);
 
-    if message.has_valid_crc::<M>() {
-        // even if the signature turn out to be invalid the valid crc shows that the received data presents a valid message as opposed to random bytes
-        reader.consume(message.raw_bytes().len());
-    } else {
-        reader.consume(1);
+    // On CRC failure, discard this whole candidate frame before searching again.
+    let has_valid_crc = message.has_valid_crc::<M>();
+    reader.consume(packet_length);
+    if !has_valid_crc {
         return Ok(None);
     }
 
@@ -1392,22 +1393,25 @@ async fn try_decode_v2_async<M: Message, R: tokio::io::AsyncRead + Unpin>(
         [..MAVLinkV2MessageRaw::HEADER_SIZE];
     message.mut_header().copy_from_slice(header);
 
+    // MAV_STX_V2 has already been consumed. Skip a rejected frame as one frame,
+    // because marker bytes inside its payload must not be treated as new packets.
+    let packet_length = message.raw_bytes().len() - 1;
     if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
-        // if there are incompatibility flags set that we do not know discard the message
+        reader.peek_exact(packet_length).await?;
+        reader.consume(packet_length);
         return Ok(None);
     }
 
-    let packet_length = message.raw_bytes().len() - 1;
     let payload_and_checksum_and_sign =
         &reader.peek_exact(packet_length).await?[MAVLinkV2MessageRaw::HEADER_SIZE..packet_length];
     message
         .mut_payload_and_checksum_and_sign()
         .copy_from_slice(payload_and_checksum_and_sign);
 
-    if message.has_valid_crc::<M>() {
-        // even if the signature turn out to be invalid the valid crc shows that the received data presents a valid message as opposed to random bytes
-        reader.consume(message.raw_bytes().len() - 1);
-    } else {
+    // On CRC failure, discard this whole candidate frame before searching again.
+    let has_valid_crc = message.has_valid_crc::<M>();
+    reader.consume(packet_length);
+    if !has_valid_crc {
         return Ok(None);
     }
 
@@ -1539,18 +1543,17 @@ pub async fn read_v2_raw_message_async<M: Message>(
             .await
             .map_err(|_| MessageReadError::Io)?;
 
-        if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
-            // if there are incompatibility flags set that we do not know discard the message
-            continue;
-        }
-
         reader
             .read_exact(message.mut_payload_and_checksum_and_sign())
             .await
             .map_err(|_| MessageReadError::Io)?;
 
-        // retry if CRC failed after previous STX
-        // (an STX byte may appear in the middle of a message)
+        // The rest of the frame has already been read. If the flags are not
+        // supported, discard this frame without scanning inside its payload.
+        if message.incompatibility_flags() & !MAVLINK_SUPPORTED_IFLAGS > 0 {
+            continue;
+        }
+
         if message.has_valid_crc::<M>() {
             return Ok(message);
         }
@@ -1768,8 +1771,6 @@ fn read_any_raw_message_inner<M: Message, R: Read>(
                     #[cfg(not(feature = "mav2-message-signing"))]
                     return Ok(MAVLinkMessageRaw::V1(message));
                 }
-
-                reader.consume(1);
             }
             MavlinkVersion::V2 => {
                 if let Some(message) = try_decode_v2::<M, _>(reader, signing_data)? {

mavlink/tests/file_connection_tests.rs

@@ -5,6 +5,8 @@ mod test_file_connections {
     use mavlink::MavConnection;
     use mavlink::dialects::ardupilotmega::MavMessage;
 
+    const ACCEPTED_LOG_MESSAGES: usize = 1374;
+
     /// Test whether we can send a message via TCP and receive it OK using async_connect.
     /// This also test signing as a property of a MavConnection if the mav2-message-signing feature is enabled.
     #[cfg(feature = "tokio")]
@@ -55,10 +57,7 @@ mod test_file_connections {
         }
 
         println!("Number of parsed messages: {counter}");
-        assert!(
-            counter == 1426,
-            "Unable to hit the necessary amount of matches"
-        );
+        assert_eq!(counter, ACCEPTED_LOG_MESSAGES);
     }
 
     #[test]
@@ -107,9 +106,6 @@ mod test_file_connections {
         }
 
         println!("Number of parsed messages: {counter}");
-        assert!(
-            counter == 1426,
-            "Unable to hit the necessary amount of matches"
-        );
+        assert_eq!(counter, ACCEPTED_LOG_MESSAGES);
     }
 }

mavlink/tests/process_log_files.rs

@@ -4,6 +4,8 @@ mod process_files {
     use mavlink::dialects::ardupilotmega::MavMessage;
     use mavlink::error::MessageReadError;
 
+    const ACCEPTED_LOG_MESSAGES: usize = 1374;
+
     #[test]
     pub fn get_file() {
         // Get path for download script
@@ -49,9 +51,6 @@ mod process_files {
         }
 
         println!("Number of parsed messages: {counter}");
-        assert!(
-            counter == 1426,
-            "Unable to hit the necessary amount of matches"
-        );
+        assert_eq!(counter, ACCEPTED_LOG_MESSAGES);
     }
 }

mavlink/tests/v1_encode_decode_tests.rs

@@ -169,4 +169,38 @@ mod test_v1_encode_decode {
         );
         assert!(buf.is_empty(), "No bytes should be written");
     }
+
+    fn corrupted_crc_packet_with_embedded_v1_frame() -> Vec<u8> {
+        let mut data = vec![mavlink::MAV_STX, HEARTBEAT_V1.len() as u8, 1, 1, 1, 0];
+        data.extend_from_slice(HEARTBEAT_V1);
+        data.extend_from_slice(&[0, 0]);
+        data
+    }
+
+    #[test]
+    pub fn test_read_v1_discards_embedded_frame_after_crc_failure() {
+        let data = corrupted_crc_packet_with_embedded_v1_frame();
+        let mut r = PeekReader::new(data.as_slice());
+
+        assert!(
+            mavlink::read_v1_msg::<mavlink::dialects::common::MavMessage, _>(&mut r).is_err(),
+            "decoded a frame embedded in a corrupt outer frame"
+        );
+    }
+
+    #[cfg(feature = "tokio")]
+    #[tokio::test]
+    pub async fn test_read_v1_async_discards_embedded_frame_after_crc_failure() {
+        use mavlink_core::async_peek_reader::AsyncPeekReader;
+
+        let data = corrupted_crc_packet_with_embedded_v1_frame();
+        let mut r = AsyncPeekReader::new(data.as_slice());
+
+        assert!(
+            mavlink::read_v1_msg_async::<mavlink::dialects::common::MavMessage, _>(&mut r)
+                .await
+                .is_err(),
+            "decoded a frame embedded in a corrupt outer frame"
+        );
+    }
 }