diff --git a/mavlink-core/src/lib.rs b/mavlink-core/src/lib.rs index 846608638ac..d0a9fbe3fe4 100644 --- a/mavlink-core/src/lib.rs +++ b/mavlink-core/src/lib.rs @@ -794,14 +794,15 @@ fn try_decode_v1( .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::() { - 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::(); + 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( .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::() { - 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::(); + 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( if let Some(msg) = try_decode_v1::(reader)? { return Ok(msg); } - - reader.consume(1); } } @@ -1347,24 +1347,25 @@ fn try_decode_v2( 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::() { - // 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::(); + reader.consume(packet_length); + if !has_valid_crc { return Ok(None); } @@ -1392,22 +1393,25 @@ async fn try_decode_v2_async( [..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::() { - // 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::(); + reader.consume(packet_length); + if !has_valid_crc { return Ok(None); } @@ -1539,18 +1543,17 @@ pub async fn read_v2_raw_message_async( .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::() { return Ok(message); } @@ -1768,8 +1771,6 @@ fn read_any_raw_message_inner( #[cfg(not(feature = "mav2-message-signing"))] return Ok(MAVLinkMessageRaw::V1(message)); } - - reader.consume(1); } MavlinkVersion::V2 => { if let Some(message) = try_decode_v2::(reader, signing_data)? { diff --git a/mavlink/tests/agnostic_decode_test.rs b/mavlink/tests/agnostic_decode_test.rs deleted file mode 100644 index bd6a26ab89e..00000000000 --- a/mavlink/tests/agnostic_decode_test.rs +++ /dev/null @@ -1,143 +0,0 @@ -pub mod test_shared; - -use mavlink::MAV_STX; -use mavlink::MAV_STX_V2; - -// 100 randomly generted bytes with 10 extra MAV_STX/MAV_STX_V2 each inserted -const GARBAGE: [u8; 120] = [ - 0xfe, 0x43, 0x2d, MAV_STX, MAV_STX, 0x26, 0x1e, 0x33, 0x85, 0x38, 0x1d, 0x20, 0x20, 0x90, 0xd9, - 0x24, 0xb6, 0xd7, 0xb1, 0x22, 0x3b, 0xaf, 0x7c, 0x2f, MAV_STX, 0x9d, 0x1a, 0x13, 0x16, 0x2b, - 0xf8, 0x6f, 0xf4, 0xdc, 0x66, 0xff, 0x2d, MAV_STX_V2, 0xe2, 0x2c, 0xb1, MAV_STX_V2, 0x4e, 0xc9, - 0xc6, 0xcb, 0x3e, 0x3e, 0xf4, MAV_STX_V2, MAV_STX_V2, 0x49, 0xbc, 0x11, 0xb7, 0xd4, 0x5e, - MAV_STX, 0x46, 0x6a, 0xd3, 0xb9, MAV_STX, 0xe3, 0x81, 0x1d, MAV_STX_V2, 0x80, 0x47, 0xfc, 0xff, - 0x0c, 0xaa, 0xf3, MAV_STX, MAV_STX_V2, 0x87, 0x2f, 0x9a, 0x15, MAV_STX_V2, MAV_STX, 0x06, 0xc9, - 0xe1, 0xc0, 0x98, 0xf5, 0x71, 0x78, 0x1c, 0x4a, 0xe3, 0xf1, MAV_STX_V2, 0x5f, 0xdb, 0x0e, 0x3f, - MAV_STX, 0x2e, MAV_STX_V2, 0x08, 0x39, 0x6e, 0x15, 0x3c, 0x55, 0xcb, 0x78, 0xe0, MAV_STX, 0x5a, - 0xb3, 0x1b, 0xf9, MAV_STX, 0xe0, 0xa0, MAV_STX_V2, -]; - -#[cfg(feature = "dialect-common")] -mod test_agnostic_encode_decode { - use crate::GARBAGE; - use mavlink_core::peek_reader::PeekReader; - use std::io::Write; - - #[test] - pub fn test_read_heartbeats() { - let mut buf = vec![]; - _ = buf.write(crate::test_shared::HEARTBEAT_V1); - _ = buf.write(crate::test_shared::HEARTBEAT_V2); - let mut r = PeekReader::new(buf.as_slice()); - // read 2 messages - for _ in 0..2 { - let (header, msg) = mavlink::read_any_msg(&mut r).expect("Failed to parse message"); - - assert_eq!(header, crate::test_shared::COMMON_MSG_HEADER); - let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); - - if let mavlink::dialects::common::MavMessage::HEARTBEAT(msg) = msg { - assert_eq!(msg.custom_mode, heartbeat_msg.custom_mode); - assert_eq!(msg.mavtype, heartbeat_msg.mavtype); - assert_eq!(msg.autopilot, heartbeat_msg.autopilot); - assert_eq!(msg.base_mode, heartbeat_msg.base_mode); - assert_eq!(msg.system_status, heartbeat_msg.system_status); - assert_eq!(msg.mavlink_version, heartbeat_msg.mavlink_version); - } else { - panic!("Decoded wrong message type") - } - } - } - - #[test] - pub fn test_read_inbetween_garbage() { - // write some garbage bytes as well as 2 heartbeats and attempt to read them - - let mut buf = vec![]; - _ = buf.write(&GARBAGE); - _ = buf.write(crate::test_shared::HEARTBEAT_V1); - _ = buf.write(&GARBAGE); - // only part of message - _ = buf.write(&crate::test_shared::HEARTBEAT_V1[..5]); - _ = buf.write(crate::test_shared::HEARTBEAT_V2); - _ = buf.write(&GARBAGE); - // only part of message - _ = buf.write(&crate::test_shared::HEARTBEAT_V1[5..]); - // add some zeros to prevent invalid package sizes from causing a read error - _ = buf.write(&[0; 100]); - - let mut r = PeekReader::new(buf.as_slice()); - _ = mavlink::read_any_msg::(&mut r).unwrap(); - _ = mavlink::read_any_msg::(&mut r).unwrap(); - assert!( - mavlink::read_any_msg::(&mut r).is_err(), - "Parsed message from garbage data" - ); - } -} - -#[cfg(all(feature = "std", feature = "tokio", feature = "dialect-common"))] -mod test_agnostic_encode_decode_async { - use crate::GARBAGE; - use mavlink_core::async_peek_reader::AsyncPeekReader; - use std::io::Write; - - #[tokio::test] - pub async fn test_read_heartbeats() { - let mut buf = vec![]; - _ = buf.write(crate::test_shared::HEARTBEAT_V1); - _ = buf.write(crate::test_shared::HEARTBEAT_V2); - let mut r = AsyncPeekReader::new(buf.as_slice()); - // read 2 messages - for _ in 0..2 { - let (header, msg) = mavlink::read_any_msg_async(&mut r) - .await - .expect("Failed to parse message"); - - assert_eq!(header, crate::test_shared::COMMON_MSG_HEADER); - let heartbeat_msg = crate::test_shared::get_heartbeat_msg(); - - if let mavlink::dialects::common::MavMessage::HEARTBEAT(msg) = msg { - assert_eq!(msg.custom_mode, heartbeat_msg.custom_mode); - assert_eq!(msg.mavtype, heartbeat_msg.mavtype); - assert_eq!(msg.autopilot, heartbeat_msg.autopilot); - assert_eq!(msg.base_mode, heartbeat_msg.base_mode); - assert_eq!(msg.system_status, heartbeat_msg.system_status); - assert_eq!(msg.mavlink_version, heartbeat_msg.mavlink_version); - } else { - panic!("Decoded wrong message type") - } - } - } - - #[tokio::test] - pub async fn test_read_inbetween_garbage() { - // write some garbage bytes as well as 2 heartbeats and attempt to read them - - let mut buf = vec![]; - _ = buf.write(&GARBAGE); - _ = buf.write(crate::test_shared::HEARTBEAT_V1); - _ = buf.write(&GARBAGE); - // only part of message - _ = buf.write(&crate::test_shared::HEARTBEAT_V1[..5]); - _ = buf.write(crate::test_shared::HEARTBEAT_V2); - _ = buf.write(&GARBAGE); - // only part of message - _ = buf.write(&crate::test_shared::HEARTBEAT_V1[5..]); - // add some zeros to prevent invalid package sizes from causing a read error - _ = buf.write(&[0; 100]); - - let mut r = AsyncPeekReader::new(buf.as_slice()); - _ = mavlink::read_any_msg_async::(&mut r) - .await - .unwrap(); - _ = mavlink::read_any_msg_async::(&mut r) - .await - .unwrap(); - assert!( - mavlink::read_any_msg_async::(&mut r) - .await - .is_err(), - "Parsed message from garbage data" - ); - } -} diff --git a/mavlink/tests/file_connection_tests.rs b/mavlink/tests/file_connection_tests.rs index bc9509dc400..8bd694d2f4d 100644 --- a/mavlink/tests/file_connection_tests.rs +++ b/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); } } diff --git a/mavlink/tests/process_log_files.rs b/mavlink/tests/process_log_files.rs index a6145f1639c..4e731c89240 100644 --- a/mavlink/tests/process_log_files.rs +++ b/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); } } diff --git a/mavlink/tests/v1_encode_decode_tests.rs b/mavlink/tests/v1_encode_decode_tests.rs index 7f59653fbe9..d5ef88ed70a 100644 --- a/mavlink/tests/v1_encode_decode_tests.rs +++ b/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 { + 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::(&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::(&mut r) + .await + .is_err(), + "decoded a frame embedded in a corrupt outer frame" + ); + } } diff --git a/mavlink/tests/v2_encode_decode_tests.rs b/mavlink/tests/v2_encode_decode_tests.rs index e9ee55d02de..3fd8ddc8ee2 100644 --- a/mavlink/tests/v2_encode_decode_tests.rs +++ b/mavlink/tests/v2_encode_decode_tests.rs @@ -222,4 +222,83 @@ mod test_v2_encode_decode { let len = mavlink::write_v2_msg(&mut out, header, &msg).expect("encode"); assert_eq!(&buffer[..len], PARAMETER_VALUE_HASH_CHECK); } + + fn corrupted_crc_packet_with_embedded_v2_frame() -> Vec { + let mut data = vec![mavlink::MAV_STX_V2, HEARTBEAT_V2.len() as u8]; + data.extend_from_slice(&[0, 0, 1, 1, 1, 0, 0, 0]); + data.extend_from_slice(HEARTBEAT_V2); + data.extend_from_slice(&[0, 0]); + data + } + + #[test] + pub fn test_read_v2_discards_embedded_frame_after_crc_failure() { + let data = corrupted_crc_packet_with_embedded_v2_frame(); + let mut r = PeekReader::new(data.as_slice()); + + assert!( + mavlink::read_v2_msg::(&mut r).is_err(), + "decoded a frame embedded in a corrupt outer frame" + ); + } + + #[cfg(feature = "tokio")] + #[tokio::test] + pub async fn test_read_v2_async_discards_embedded_frame_after_crc_failure() { + use mavlink_core::async_peek_reader::AsyncPeekReader; + + let data = corrupted_crc_packet_with_embedded_v2_frame(); + let mut r = AsyncPeekReader::new(data.as_slice()); + + assert!( + mavlink::read_v2_msg_async::(&mut r) + .await + .is_err(), + "decoded a frame embedded in a corrupt outer frame" + ); + } + + #[test] + pub fn test_read_v2_rejects_packet_in_packet_injection() { + let next_header = mavlink::MavHeader { + sequence: 161, + system_id: crate::test_shared::COMMON_MSG_HEADER.system_id, + component_id: crate::test_shared::COMMON_MSG_HEADER.component_id, + }; + let next_msg = mavlink::dialects::common::MavMessage::HEARTBEAT( + crate::test_shared::get_heartbeat_msg(), + ); + let mut next_frame = Vec::new(); + mavlink::write_v2_msg(&mut next_frame, next_header, &next_msg).expect("encode"); + + // Try to inject a heartbeat immediately after a fake one byte payload. + let mut data = vec![ + mavlink::MAV_STX_V2, // fake marker + 1, // fake payload length + 0, // fake incompatibility flags + 0, // fake compatibility flags + 1, // fake sequence + 1, // fake system id + 1, // fake component id + 0, // fake message id byte 0 + 0, // fake message id byte 1 + 0, // fake message id byte 2 + 0, // fake payload byte 0 + ]; + data.extend_from_slice(HEARTBEAT_V2); + data.extend_from_slice(&next_frame); + + let mut r = PeekReader::new(data.as_slice()); + let (header, msg) = + mavlink::read_v2_msg::(&mut r) + .expect("did not reach the valid frame after the injection attempt"); + + // Proves the parser skipped the rejected fake frame and the injected frame. + assert_eq!(header, next_header); + // Proves the parser still accepts the next valid frame in the stream. + assert!(matches!( + msg, + mavlink::dialects::common::MavMessage::HEARTBEAT(_) + )); + } }