use crate::error::FrameError;
const INCOMPAT_FLAG_SIGNED: u8 = 0x01;
const V1_MAGIC: u8 = 0xFE;
const V2_MAGIC: u8 = 0xFD;
const V1_HEADER_LEN: usize = 6;
const V2_HEADER_LEN: usize = 10;
pub const MAX_V2_FRAME_LEN: usize = V2_HEADER_LEN + 255 + 2;
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub enum MavVersion {
V1,
V2,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
pub struct Frame<'a> {
version: MavVersion,
incompat_flags: u8,
compat_flags: u8,
sequence: u8,
system_id: u8,
component_id: u8,
message_id: u32,
payload: &'a [u8],
checksum: u16,
header_and_payload: &'a [u8],
}
impl<'a> Frame<'a> {
pub fn parse(buf: &'a [u8]) -> Result<Self, FrameError> {
match buf.first() {
None => Err(FrameError::TooShort { needed: 1, got: 0 }),
Some(&V1_MAGIC) => Self::parse_v1(buf),
Some(&V2_MAGIC) => Self::parse_v2(buf),
Some(&other) => Err(FrameError::UnknownMagicByte(other)),
}
}
fn parse_v1(buf: &'a [u8]) -> Result<Self, FrameError> {
if buf.len() < V1_HEADER_LEN {
return Err(FrameError::TooShort {
needed: V1_HEADER_LEN,
got: buf.len(),
});
}
let len = usize::from(buf[1]);
let needed = V1_HEADER_LEN + len + 2;
if buf.len() < needed {
return Err(FrameError::TooShort {
needed,
got: buf.len(),
});
}
Ok(Self {
version: MavVersion::V1,
incompat_flags: 0,
compat_flags: 0,
sequence: buf[2],
system_id: buf[3],
component_id: buf[4],
message_id: u32::from(buf[5]),
payload: &buf[V1_HEADER_LEN..V1_HEADER_LEN + len],
checksum: u16::from_le_bytes([buf[V1_HEADER_LEN + len], buf[V1_HEADER_LEN + len + 1]]),
header_and_payload: &buf[1..V1_HEADER_LEN + len],
})
}
fn parse_v2(buf: &'a [u8]) -> Result<Self, FrameError> {
if buf.len() < V2_HEADER_LEN {
return Err(FrameError::TooShort {
needed: V2_HEADER_LEN,
got: buf.len(),
});
}
let incompat_flags = buf[2];
if incompat_flags & INCOMPAT_FLAG_SIGNED != 0 {
return Err(FrameError::SigningNotSupported);
}
let len = usize::from(buf[1]);
let needed = V2_HEADER_LEN + len + 2;
if buf.len() < needed {
return Err(FrameError::TooShort {
needed,
got: buf.len(),
});
}
let message_id = u32::from(buf[7]) | (u32::from(buf[8]) << 8) | (u32::from(buf[9]) << 16);
Ok(Self {
version: MavVersion::V2,
incompat_flags,
compat_flags: buf[3],
sequence: buf[4],
system_id: buf[5],
component_id: buf[6],
message_id,
payload: &buf[V2_HEADER_LEN..V2_HEADER_LEN + len],
checksum: u16::from_le_bytes([buf[V2_HEADER_LEN + len], buf[V2_HEADER_LEN + len + 1]]),
header_and_payload: &buf[1..V2_HEADER_LEN + len],
})
}
#[must_use]
pub const fn version(&self) -> MavVersion {
self.version
}
#[must_use]
pub const fn incompat_flags(&self) -> u8 {
self.incompat_flags
}
#[must_use]
pub const fn compat_flags(&self) -> u8 {
self.compat_flags
}
#[must_use]
pub const fn sequence(&self) -> u8 {
self.sequence
}
#[must_use]
pub const fn system_id(&self) -> u8 {
self.system_id
}
#[must_use]
pub const fn component_id(&self) -> u8 {
self.component_id
}
#[must_use]
pub const fn message_id(&self) -> u32 {
self.message_id
}
#[must_use]
pub const fn payload(&self) -> &'a [u8] {
self.payload
}
#[must_use]
pub const fn checksum(&self) -> u16 {
self.checksum
}
#[must_use]
pub const fn byte_len(&self) -> usize {
1 + self.header_and_payload.len() + 2
}
#[must_use]
pub fn verify_checksum(&self, crc_extra: u8) -> bool {
crate::crc::checksum(self.header_and_payload, crc_extra) == self.checksum
}
}
pub fn encode_v2(
buf: &mut [u8],
system_id: u8,
component_id: u8,
sequence: u8,
message_id: u32,
payload: &[u8],
crc_extra: u8,
) -> Result<usize, FrameError> {
if payload.len() > 255 {
return Err(FrameError::PayloadTooLong {
max: 255,
got: payload.len(),
});
}
let mut len = payload.len();
while len > 1 && payload[len - 1] == 0 {
len -= 1;
}
let truncated = &payload[..len];
let total = V2_HEADER_LEN + len + 2;
if buf.len() < total {
return Err(FrameError::TooShort {
needed: total,
got: buf.len(),
});
}
buf[0] = V2_MAGIC;
#[allow(
clippy::cast_possible_truncation,
reason = "len <= payload.len() <= 255, checked above"
)]
{
buf[1] = len as u8;
}
buf[2] = 0; buf[3] = 0; buf[4] = sequence;
buf[5] = system_id;
buf[6] = component_id;
#[allow(
clippy::cast_possible_truncation,
reason = "message_id is documented as fitting 24 bits; only the low 3 bytes are used"
)]
{
buf[7] = message_id as u8;
buf[8] = (message_id >> 8) as u8;
buf[9] = (message_id >> 16) as u8;
}
buf[V2_HEADER_LEN..V2_HEADER_LEN + len].copy_from_slice(truncated);
let crc = crate::crc::checksum(&buf[1..V2_HEADER_LEN + len], crc_extra);
buf[V2_HEADER_LEN + len..total].copy_from_slice(&crc.to_le_bytes());
Ok(total)
}
#[cfg(kani)]
mod kani_proofs {
use super::{Frame, encode_v2};
#[kani::proof]
fn parse_never_panics() {
let buf: [u8; 16] = kani::any();
let _ = Frame::parse(&buf);
}
#[kani::proof]
#[kani::unwind(20)]
fn encode_v2_never_panics() {
let mut buf = [0u8; super::MAX_V2_FRAME_LEN];
let payload: [u8; 8] = kani::any();
let system_id: u8 = kani::any();
let component_id: u8 = kani::any();
let sequence: u8 = kani::any();
let message_id: u32 = kani::any();
let crc_extra: u8 = kani::any();
let _ = encode_v2(
&mut buf,
system_id,
component_id,
sequence,
message_id,
&payload,
crc_extra,
);
}
#[kani::proof]
#[kani::unwind(13)]
fn encode_then_parse_round_trips() {
let mut buf = [0u8; super::MAX_V2_FRAME_LEN];
let payload: [u8; 1] = kani::any();
let system_id: u8 = kani::any();
let component_id: u8 = kani::any();
let sequence: u8 = kani::any();
let message_id: u32 = kani::any();
kani::assume(message_id <= 0x00FF_FFFF);
let crc_extra: u8 = kani::any();
let n = encode_v2(
&mut buf,
system_id,
component_id,
sequence,
message_id,
&payload,
crc_extra,
)
.unwrap();
let frame = Frame::parse(&buf[..n]).unwrap();
assert_eq!(frame.system_id(), system_id);
assert_eq!(frame.component_id(), component_id);
assert_eq!(frame.sequence(), sequence);
assert_eq!(frame.message_id(), message_id);
assert!(frame.verify_checksum(crc_extra));
}
}
#[cfg(test)]
mod tests {
use super::*;
fn hex_decode(hex: &str) -> [u8; 32] {
let mut out = [0u8; 32];
for (i, chunk) in hex.as_bytes().chunks(2).enumerate() {
let s = core::str::from_utf8(chunk).unwrap();
out[i] = u8::from_str_radix(s, 16).unwrap();
}
out
}
#[test]
fn parses_a_real_captured_heartbeat_v2_frame() {
let hex = "fd090000000101000000000000000203510403e71e";
let buf = hex_decode(hex);
let frame = Frame::parse(&buf[..hex.len() / 2]).unwrap();
assert_eq!(frame.version(), MavVersion::V2);
assert_eq!(frame.system_id(), 1);
assert_eq!(frame.component_id(), 1);
assert_eq!(frame.sequence(), 0);
assert_eq!(frame.message_id(), 0);
assert_eq!(frame.payload().len(), 9);
assert!(frame.verify_checksum(50));
assert!(!frame.verify_checksum(51));
}
#[test]
fn parses_a_truncated_v2_statustext_frame() {
let hex = "fd060000000101fd00000668656c6c6f4795";
let buf = hex_decode(hex);
let frame = Frame::parse(&buf[..hex.len() / 2]).unwrap();
assert_eq!(frame.message_id(), 253);
assert_eq!(frame.payload(), b"\x06hello");
assert!(frame.verify_checksum(83));
}
#[test]
fn rejects_a_signed_v2_frame() {
let mut buf = hex_decode("fd090000000101000000000000000203510403e71e");
buf[2] = INCOMPAT_FLAG_SIGNED;
assert_eq!(
Frame::parse(&buf[..21]),
Err(FrameError::SigningNotSupported)
);
}
#[test]
fn rejects_an_unknown_magic_byte() {
assert_eq!(
Frame::parse(&[0x00, 0x00]),
Err(FrameError::UnknownMagicByte(0x00))
);
}
#[test]
fn rejects_a_buffer_shorter_than_the_declared_length() {
let buf = hex_decode("fd090000000101000000000000000203510403e71e");
assert_eq!(
Frame::parse(&buf[..10]),
Err(FrameError::TooShort {
needed: 21,
got: 10
})
);
}
#[test]
fn encode_v2_matches_a_real_pymavlink_heartbeat_frame() {
let mut buf = [0u8; MAX_V2_FRAME_LEN];
let payload = [0u8, 0, 0, 0, 2, 3, 0x51, 4, 3]; let n = encode_v2(&mut buf, 1, 1, 0, 0, &payload, 50).unwrap();
let expected = hex_decode("fd090000000101000000000000000203510403e71e");
assert_eq!(&buf[..n], &expected[..n]);
}
#[test]
fn encode_v2_strips_trailing_zeros_like_a_real_statustext_frame() {
let mut buf = [0u8; MAX_V2_FRAME_LEN];
let mut payload = [0u8; 54];
payload[0] = 6; payload[1..6].copy_from_slice(b"hello");
let n = encode_v2(&mut buf, 1, 1, 0, 253, &payload, 83).unwrap();
let expected = hex_decode("fd060000000101fd00000668656c6c6f4795");
assert_eq!(&buf[..n], &expected[..n]);
}
#[test]
fn encode_v2_round_trips_through_parse() {
let mut buf = [0u8; MAX_V2_FRAME_LEN];
let payload = [1u8, 2, 3, 4, 5];
let n = encode_v2(&mut buf, 7, 8, 9, 12345, &payload, 200).unwrap();
let frame = Frame::parse(&buf[..n]).unwrap();
assert_eq!(frame.system_id(), 7);
assert_eq!(frame.component_id(), 8);
assert_eq!(frame.sequence(), 9);
assert_eq!(frame.message_id(), 12345);
assert_eq!(frame.payload(), &payload);
assert!(frame.verify_checksum(200));
}
#[test]
fn encode_v2_truncates_a_message_id_wider_than_24_bits() {
let mut buf = [0u8; MAX_V2_FRAME_LEN];
let n = encode_v2(&mut buf, 1, 1, 0, 0xAB00_1234, &[0], 0).unwrap();
let frame = Frame::parse(&buf[..n]).unwrap();
assert_eq!(frame.message_id(), 0x00_1234);
}
#[test]
fn byte_len_matches_the_bytes_actually_consumed() {
let mut buf = [0u8; MAX_V2_FRAME_LEN];
let payload = [1u8, 2, 3, 4, 5];
let n = encode_v2(&mut buf, 1, 1, 0, 12345, &payload, 200).unwrap();
let frame = Frame::parse(&buf[..n]).unwrap();
assert_eq!(frame.byte_len(), n);
}
}