1use crate::crc::checksum;
5use crate::dialect::Message;
6use crate::error::{MavlinkError, Result};
7
8pub const MAGIC_V1: u8 = 0xFE;
10pub const MAGIC_V2: u8 = 0xFD;
12
13pub const IFLAG_SIGNED: u8 = 0x01;
15
16pub const MAX_PAYLOAD: usize = 255;
18pub const SIGNATURE_LEN: usize = 13;
20
21const HEADER_V1: usize = 6;
23const HEADER_V2: usize = 10;
24const CHECKSUM_LEN: usize = 2;
25
26pub const MAX_FRAME: usize = HEADER_V2 + MAX_PAYLOAD + CHECKSUM_LEN + SIGNATURE_LEN;
29
30#[derive(Clone, Copy, Debug, PartialEq, Eq)]
32pub enum Version {
33 V1,
35 V2,
38}
39
40#[derive(Clone, Copy, Debug, PartialEq, Eq)]
46pub struct Header {
47 pub system_id: u8,
49 pub component_id: u8,
51 pub sequence: u8,
53}
54
55impl Header {
56 pub const fn new(system_id: u8, component_id: u8, sequence: u8) -> Self {
68 Header {
69 system_id,
70 component_id,
71 sequence,
72 }
73 }
74}
75
76#[derive(Clone, Copy, Debug, PartialEq, Eq)]
84pub struct Frame {
85 bytes: [u8; MAX_FRAME],
86 len: usize,
87}
88
89impl Frame {
90 const fn header_len(version: Version) -> usize {
92 match version {
93 Version::V1 => HEADER_V1,
94 Version::V2 => HEADER_V2,
95 }
96 }
97
98 pub fn encode_v2(header: Header, msgid: u32, payload: &[u8], crc_extra: u8) -> Result<Frame> {
119 Self::assemble_v2(header, msgid, payload, crc_extra, 0)
120 }
121
122 pub fn encode_message<M: Message>(header: Header, message: &M) -> Result<Frame> {
141 let mut payload = [0u8; MAX_PAYLOAD];
142 let len = message.encode(&mut payload);
143 Self::encode_v2(header, M::ID, &payload[..len], M::CRC_EXTRA)
144 }
145
146 pub fn decode_message<M: Message>(&self) -> Result<M> {
157 M::decode(self.payload())
158 }
159
160 pub(crate) fn assemble_v2(
163 header: Header,
164 msgid: u32,
165 payload: &[u8],
166 crc_extra: u8,
167 incompat_flags: u8,
168 ) -> Result<Frame> {
169 if payload.len() > MAX_PAYLOAD {
170 return Err(MavlinkError::PayloadTooLong);
171 }
172 let plen = truncated_len(payload);
173 let signed = incompat_flags & IFLAG_SIGNED != 0;
174 let total = HEADER_V2 + plen + CHECKSUM_LEN + if signed { SIGNATURE_LEN } else { 0 };
175
176 let mut bytes = [0u8; MAX_FRAME];
177 bytes[0] = MAGIC_V2;
178 bytes[1] = plen as u8;
179 bytes[2] = incompat_flags;
180 bytes[3] = 0;
181 bytes[4] = header.sequence;
182 bytes[5] = header.system_id;
183 bytes[6] = header.component_id;
184 bytes[7] = msgid as u8;
185 bytes[8] = (msgid >> 8) as u8;
186 bytes[9] = (msgid >> 16) as u8;
187 bytes[HEADER_V2..HEADER_V2 + plen].copy_from_slice(&payload[..plen]);
188
189 let crc = checksum(&bytes[1..HEADER_V2 + plen], crc_extra);
190 bytes[HEADER_V2 + plen..HEADER_V2 + plen + CHECKSUM_LEN]
191 .copy_from_slice(&crc.to_le_bytes());
192
193 Ok(Frame { bytes, len: total })
194 }
195
196 pub fn encode_v1(header: Header, msgid: u32, payload: &[u8], crc_extra: u8) -> Result<Frame> {
214 if payload.len() > MAX_PAYLOAD {
215 return Err(MavlinkError::PayloadTooLong);
216 }
217 if msgid > 0xFF {
218 return Err(MavlinkError::UnknownMessage(msgid));
219 }
220 let plen = payload.len();
221 let total = HEADER_V1 + plen + CHECKSUM_LEN;
222
223 let mut bytes = [0u8; MAX_FRAME];
224 bytes[0] = MAGIC_V1;
225 bytes[1] = plen as u8;
226 bytes[2] = header.sequence;
227 bytes[3] = header.system_id;
228 bytes[4] = header.component_id;
229 bytes[5] = msgid as u8;
230 bytes[HEADER_V1..HEADER_V1 + plen].copy_from_slice(payload);
231
232 let crc = checksum(&bytes[1..HEADER_V1 + plen], crc_extra);
233 bytes[HEADER_V1 + plen..HEADER_V1 + plen + CHECKSUM_LEN]
234 .copy_from_slice(&crc.to_le_bytes());
235
236 Ok(Frame { bytes, len: total })
237 }
238
239 pub fn parse_with<F>(bytes: &[u8], crc_extra_for: F) -> Result<Frame>
264 where
265 F: FnOnce(u32) -> Option<u8>,
266 {
267 if bytes.is_empty() {
268 return Err(MavlinkError::FrameTooShort);
269 }
270 let version = match bytes[0] {
271 MAGIC_V1 => Version::V1,
272 MAGIC_V2 => Version::V2,
273 other => return Err(MavlinkError::BadMagic(other)),
274 };
275 let header_len = Self::header_len(version);
276 if bytes.len() < header_len {
277 return Err(MavlinkError::FrameTooShort);
278 }
279 if version == Version::V2 && bytes[2] & !IFLAG_SIGNED != 0 {
280 return Err(MavlinkError::UnknownIncompatFlags(bytes[2]));
281 }
282 let plen = bytes[1] as usize;
283 let signed = version == Version::V2 && bytes[2] & IFLAG_SIGNED != 0;
284 let total = header_len + plen + CHECKSUM_LEN + if signed { SIGNATURE_LEN } else { 0 };
285 if bytes.len() < total {
286 return Err(MavlinkError::Truncated);
287 }
288
289 let msgid = match version {
290 Version::V1 => u32::from(bytes[5]),
291 Version::V2 => {
292 u32::from(bytes[7]) | u32::from(bytes[8]) << 8 | u32::from(bytes[9]) << 16
293 }
294 };
295 let crc_extra = crc_extra_for(msgid).ok_or(MavlinkError::UnknownMessage(msgid))?;
296
297 let crc_at = header_len + plen;
298 let expected = checksum(&bytes[1..crc_at], crc_extra);
299 let found = u16::from_le_bytes([bytes[crc_at], bytes[crc_at + 1]]);
300 if expected != found {
301 return Err(MavlinkError::CrcMismatch { expected, found });
302 }
303
304 let mut buffer = [0u8; MAX_FRAME];
305 buffer[..total].copy_from_slice(&bytes[..total]);
306 Ok(Frame {
307 bytes: buffer,
308 len: total,
309 })
310 }
311
312 pub fn parse(bytes: &[u8], crc_extra: u8) -> Result<Frame> {
327 Self::parse_with(bytes, |_| Some(crc_extra))
328 }
329
330 pub fn version(&self) -> Version {
336 if self.bytes[0] == MAGIC_V2 {
337 Version::V2
338 } else {
339 Version::V1
340 }
341 }
342
343 pub fn sequence(&self) -> u8 {
349 match self.version() {
350 Version::V1 => self.bytes[2],
351 Version::V2 => self.bytes[4],
352 }
353 }
354
355 pub fn system_id(&self) -> u8 {
361 match self.version() {
362 Version::V1 => self.bytes[3],
363 Version::V2 => self.bytes[5],
364 }
365 }
366
367 pub fn component_id(&self) -> u8 {
373 match self.version() {
374 Version::V1 => self.bytes[4],
375 Version::V2 => self.bytes[6],
376 }
377 }
378
379 pub fn message_id(&self) -> u32 {
385 match self.version() {
386 Version::V1 => u32::from(self.bytes[5]),
387 Version::V2 => {
388 u32::from(self.bytes[7])
389 | u32::from(self.bytes[8]) << 8
390 | u32::from(self.bytes[9]) << 16
391 }
392 }
393 }
394
395 pub fn incompat_flags(&self) -> u8 {
401 match self.version() {
402 Version::V1 => 0,
403 Version::V2 => self.bytes[2],
404 }
405 }
406
407 pub fn is_signed(&self) -> bool {
413 self.version() == Version::V2 && self.incompat_flags() & IFLAG_SIGNED != 0
414 }
415
416 pub fn payload(&self) -> &[u8] {
422 let start = Self::header_len(self.version());
423 let plen = self.bytes[1] as usize;
424 &self.bytes[start..start + plen]
425 }
426
427 pub fn signature(&self) -> Option<&[u8; SIGNATURE_LEN]> {
433 if !self.is_signed() {
434 return None;
435 }
436 let start = HEADER_V2 + self.bytes[1] as usize + CHECKSUM_LEN;
437 self.bytes[start..start + SIGNATURE_LEN].try_into().ok()
438 }
439
440 pub fn as_bytes(&self) -> &[u8] {
446 &self.bytes[..self.len]
447 }
448
449 pub(crate) fn signed_region(&self) -> &[u8] {
452 let start = HEADER_V2 + self.bytes[1] as usize + CHECKSUM_LEN;
453 &self.bytes[..start]
454 }
455
456 pub(crate) fn signature_mut(&mut self) -> &mut [u8; SIGNATURE_LEN] {
458 let start = HEADER_V2 + self.bytes[1] as usize + CHECKSUM_LEN;
459 (&mut self.bytes[start..start + SIGNATURE_LEN])
460 .try_into()
461 .expect("a signed frame reserves a full signature block")
462 }
463}
464
465fn truncated_len(payload: &[u8]) -> usize {
469 let mut len = payload.len();
470 while len > 1 && payload[len - 1] == 0 {
471 len -= 1;
472 }
473 len
474}
475
476#[cfg(test)]
477mod tests {
478 use super::*;
479
480 const HEARTBEAT_ID: u32 = 0;
482 const HEARTBEAT_CRC_EXTRA: u8 = 50;
483
484 #[test]
488 fn a_frame_with_an_incompatibility_flag_it_does_not_know_is_dropped() {
489 let payload = [0x06, 0x08, 0x00, 0x00, 0x00, 0x02, 0x03, 0x59, 0x03];
490 let flagged = Frame::assemble_v2(
491 Header::new(1, 1, 0),
492 HEARTBEAT_ID,
493 &payload,
494 HEARTBEAT_CRC_EXTRA,
495 0x02,
496 )
497 .unwrap();
498 assert_eq!(
499 Frame::parse(flagged.as_bytes(), HEARTBEAT_CRC_EXTRA),
500 Err(MavlinkError::UnknownIncompatFlags(0x02))
501 );
502 let good = Frame::encode_v2(
503 Header::new(1, 1, 1),
504 HEARTBEAT_ID,
505 &payload,
506 HEARTBEAT_CRC_EXTRA,
507 )
508 .unwrap();
509 let mut parser = crate::Parser::new();
510 let heard: Vec<Frame> = [flagged.as_bytes(), good.as_bytes()]
511 .concat()
512 .iter()
513 .filter_map(|&byte| parser.push_byte(byte, &|_| Some(HEARTBEAT_CRC_EXTRA)))
514 .collect();
515 assert_eq!(heard.len(), 1, "the parser passes over the flagged frame");
516 assert_eq!(heard[0].sequence(), 1);
517 }
518
519 #[test]
520 fn a_v2_frame_round_trips_through_parse() {
521 let header = Header::new(1, 1, 7);
522 let payload = [0x06, 0x08, 0x00, 0x00, 0x00, 0x02, 0x03, 0x59, 0x03];
523 let frame = Frame::encode_v2(header, HEARTBEAT_ID, &payload, HEARTBEAT_CRC_EXTRA).unwrap();
524 let parsed = Frame::parse(frame.as_bytes(), HEARTBEAT_CRC_EXTRA).unwrap();
525 assert_eq!(parsed.version(), Version::V2);
526 assert_eq!(parsed.message_id(), HEARTBEAT_ID);
527 assert_eq!(parsed.system_id(), 1);
528 assert_eq!(parsed.sequence(), 7);
529 assert_eq!(parsed.payload(), &payload);
530 }
531
532 #[test]
533 fn a_v1_frame_round_trips_through_parse() {
534 let header = Header::new(1, 1, 0);
535 let payload = [0x06, 0x08, 0x00, 0x00, 0x00, 0x02, 0x03, 0x59, 0x03];
536 let frame = Frame::encode_v1(header, HEARTBEAT_ID, &payload, HEARTBEAT_CRC_EXTRA).unwrap();
537 let parsed = Frame::parse(frame.as_bytes(), HEARTBEAT_CRC_EXTRA).unwrap();
538 assert_eq!(parsed.version(), Version::V1);
539 assert_eq!(parsed.message_id(), HEARTBEAT_ID);
540 assert_eq!(parsed.payload(), &payload);
541 }
542
543 #[test]
544 fn the_v2_header_is_laid_out_as_the_spec_requires() {
545 let header = Header::new(0x2A, 0xBE, 0x10);
546 let frame = Frame::encode_v2(header, 0x0A0B0C, &[1, 2, 3], 0).unwrap();
547 let bytes = frame.as_bytes();
548 assert_eq!(bytes[0], MAGIC_V2);
549 assert_eq!(bytes[1], 3); assert_eq!(bytes[2], 0); assert_eq!(bytes[3], 0); assert_eq!(bytes[4], 0x10); assert_eq!(bytes[5], 0x2A); assert_eq!(bytes[6], 0xBE); assert_eq!(&bytes[7..10], &[0x0C, 0x0B, 0x0A]); }
557
558 #[test]
559 fn trailing_zero_bytes_are_truncated_but_the_first_is_kept() {
560 let header = Header::new(1, 1, 0);
561 let frame = Frame::encode_v2(header, 0, &[0, 0, 0, 0], 50).unwrap();
563 assert_eq!(frame.payload(), &[0]);
564
565 let frame = Frame::encode_v2(header, 0, &[1, 0, 2, 0, 0], 50).unwrap();
567 assert_eq!(frame.payload(), &[1, 0, 2]);
568 }
569
570 #[test]
571 fn a_corrupt_checksum_is_rejected() {
572 let header = Header::new(1, 1, 0);
573 let frame = Frame::encode_v2(header, 0, &[1, 2, 3], 50).unwrap();
574 let mut bytes = frame.as_bytes().to_vec();
575 let last = bytes.len() - 1;
576 bytes[last] ^= 0xFF;
577 assert!(matches!(
578 Frame::parse(&bytes, 50),
579 Err(MavlinkError::CrcMismatch { .. })
580 ));
581 }
582
583 #[test]
584 fn the_wrong_crc_extra_is_rejected() {
585 let header = Header::new(1, 1, 0);
588 let frame = Frame::encode_v2(header, 0, &[1, 2, 3], 50).unwrap();
589 assert!(matches!(
590 Frame::parse(frame.as_bytes(), 51),
591 Err(MavlinkError::CrcMismatch { .. })
592 ));
593 }
594
595 #[test]
596 fn an_unknown_message_is_rejected() {
597 let header = Header::new(1, 1, 0);
598 let frame = Frame::encode_v2(header, 999, &[1, 2, 3], 50).unwrap();
599 assert_eq!(
600 Frame::parse_with(frame.as_bytes(), |_| None),
601 Err(MavlinkError::UnknownMessage(999))
602 );
603 }
604
605 #[test]
606 fn a_truncated_frame_is_rejected() {
607 let header = Header::new(1, 1, 0);
608 let frame = Frame::encode_v2(header, 0, &[1, 2, 3, 4, 5], 50).unwrap();
609 let bytes = frame.as_bytes();
610 assert_eq!(
611 Frame::parse(&bytes[..bytes.len() - 1], 50),
612 Err(MavlinkError::Truncated)
613 );
614 }
615
616 #[test]
617 fn an_unrecognized_start_marker_is_rejected() {
618 assert_eq!(
619 Frame::parse(&[0x00, 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07], 50),
620 Err(MavlinkError::BadMagic(0x00))
621 );
622 }
623}