363 lines
11 KiB
Rust
363 lines
11 KiB
Rust
//! MAVLink serial-byte receive API.
|
|
|
|
use crate::{MavlinkFrame, decode_message, generated::kaptan::MavMessage};
|
|
|
|
/// An unsigned MAVLink 2 frame whose ID and CRC match the Kaptan dialect.
|
|
///
|
|
/// The payload is deliberately not decoded during byte admission. Consumers
|
|
/// decode it once at protocol dispatch.
|
|
#[repr(transparent)]
|
|
pub struct IntegrityCheckedFrame(MavlinkFrame);
|
|
|
|
const _: [(); core::mem::size_of::<MavlinkFrame>()] =
|
|
[(); core::mem::size_of::<IntegrityCheckedFrame>()];
|
|
const _: [(); core::mem::align_of::<MavlinkFrame>()] =
|
|
[(); core::mem::align_of::<IntegrityCheckedFrame>()];
|
|
|
|
impl IntegrityCheckedFrame {
|
|
fn try_new(frame: MavlinkFrame) -> Option<Self> {
|
|
let message_id = frame.message_id();
|
|
(MavMessage::all_ids().contains(&message_id) && frame.has_valid_crc::<MavMessage>())
|
|
.then_some(Self(frame))
|
|
}
|
|
|
|
/// Returns the admitted raw frame without copying it.
|
|
pub const fn raw(&self) -> &MavlinkFrame {
|
|
&self.0
|
|
}
|
|
|
|
/// Decodes the admitted Kaptan payload without repeating its CRC check.
|
|
pub fn decode(&self) -> Option<MavMessage> {
|
|
decode_message(&self.0)
|
|
}
|
|
}
|
|
|
|
/// Incremental, unbuffered MAVLink 2 framing with Kaptan ID and CRC validation.
|
|
///
|
|
/// Consumers that can pause byte admission can process each returned frame
|
|
/// directly. Interrupt-driven byte streams can use [`IntegrityMavlinkReceiver`]
|
|
/// to queue complete frames instead. Accepted frames remain unauthenticated.
|
|
pub struct IntegrityMavlinkFramer {
|
|
frame: MavlinkFrame,
|
|
received: usize,
|
|
expected: usize,
|
|
}
|
|
|
|
impl IntegrityMavlinkFramer {
|
|
/// Creates an empty framer.
|
|
pub const fn new() -> Self {
|
|
Self {
|
|
frame: MavlinkFrame::new(),
|
|
received: 0,
|
|
expected: 0,
|
|
}
|
|
}
|
|
|
|
/// Consumes one byte, returning a complete integrity-checked frame if ready.
|
|
pub fn receive_byte(&mut self, byte: u8) -> Option<IntegrityCheckedFrame> {
|
|
if self.received == 0 {
|
|
if byte == 0xfd {
|
|
self.frame.as_mut_slice()[0] = byte;
|
|
self.received = 1;
|
|
}
|
|
return None;
|
|
}
|
|
|
|
self.frame.as_mut_slice()[self.received] = byte;
|
|
self.received += 1;
|
|
|
|
if self.received == 10 {
|
|
let header = self.frame.as_slice();
|
|
// MAVLink incompatibility bit zero adds a signature. This framer
|
|
// has neither a signing verifier nor persistent replay state, so
|
|
// every nonzero incompatibility flag is rejected at the header.
|
|
if header[2] != 0 {
|
|
self.reset();
|
|
return None;
|
|
}
|
|
self.expected = 1 + 9 + usize::from(header[1]) + 2;
|
|
}
|
|
|
|
if self.expected == 0 || self.received != self.expected {
|
|
return None;
|
|
}
|
|
|
|
let frame = core::mem::replace(&mut self.frame, MavlinkFrame::new());
|
|
self.received = 0;
|
|
self.expected = 0;
|
|
IntegrityCheckedFrame::try_new(frame)
|
|
}
|
|
|
|
/// Discards a partial frame after a stream discontinuity.
|
|
pub fn reset(&mut self) {
|
|
self.frame = MavlinkFrame::new();
|
|
self.received = 0;
|
|
self.expected = 0;
|
|
}
|
|
}
|
|
|
|
impl Default for IntegrityMavlinkFramer {
|
|
fn default() -> Self {
|
|
Self::new()
|
|
}
|
|
}
|
|
|
|
/// Bounded integrity-only MAVLink 2 receiver.
|
|
///
|
|
/// It rejects signed, incompatible, corrupt, and MAVLink 1 frames, but an
|
|
/// accepted frame is still unauthenticated. The composition decides whether
|
|
/// and how a received request may be admitted.
|
|
pub struct IntegrityMavlinkReceiver<const N: usize> {
|
|
framer: IntegrityMavlinkFramer,
|
|
frames: [Option<IntegrityCheckedFrame>; N],
|
|
front: usize,
|
|
length: usize,
|
|
}
|
|
|
|
impl<const N: usize> IntegrityMavlinkReceiver<N> {
|
|
/// Creates an empty integrity-only receiver.
|
|
pub const fn new() -> Self {
|
|
assert!(N != 0, "integrity receiver capacity must be nonzero");
|
|
Self {
|
|
framer: IntegrityMavlinkFramer::new(),
|
|
frames: [const { None }; N],
|
|
front: 0,
|
|
length: 0,
|
|
}
|
|
}
|
|
|
|
/// Queues one complete CRC-valid unsigned frame when capacity remains.
|
|
pub fn receive_byte(&mut self, byte: u8) -> bool {
|
|
let Some(frame) = self.framer.receive_byte(byte) else {
|
|
return false;
|
|
};
|
|
if self.length == N {
|
|
return false;
|
|
}
|
|
let index = (self.front + self.length) % N;
|
|
self.frames[index] = Some(frame);
|
|
self.length += 1;
|
|
true
|
|
}
|
|
|
|
/// Discards partial framing state and all queued unauthenticated frames.
|
|
pub fn reset(&mut self) {
|
|
self.framer.reset();
|
|
self.frames = [const { None }; N];
|
|
self.front = 0;
|
|
self.length = 0;
|
|
}
|
|
|
|
/// Takes the oldest integrity-checked frame.
|
|
pub fn take_received(&mut self) -> Option<IntegrityCheckedFrame> {
|
|
if self.length == 0 {
|
|
return None;
|
|
}
|
|
let frame = self.frames[self.front].take();
|
|
self.front = (self.front + 1) % N;
|
|
self.length -= 1;
|
|
frame
|
|
}
|
|
}
|
|
|
|
impl<const N: usize> Default for IntegrityMavlinkReceiver<N> {
|
|
fn default() -> Self {
|
|
Self::new()
|
|
}
|
|
}
|
|
|
|
#[cfg(test)]
|
|
mod tests {
|
|
use super::{IntegrityCheckedFrame, IntegrityMavlinkReceiver};
|
|
use crate::{
|
|
MavlinkFrame,
|
|
generated::kaptan::{HEARTBEAT_DATA, HeartbeatState, MavMessage},
|
|
};
|
|
use mavlink_core::{
|
|
MAVLinkV1MessageRaw, MavHeader, MavlinkVersion, MessageData, error::ParserError,
|
|
};
|
|
|
|
struct ForeignHeartbeat;
|
|
|
|
impl MessageData for ForeignHeartbeat {
|
|
type Message = MavMessage;
|
|
|
|
const ID: u32 = 0;
|
|
const NAME: &'static str = "FOREIGN_HEARTBEAT";
|
|
const EXTRA_CRC: u8 = 50;
|
|
const ENCODED_LEN: usize = 9;
|
|
|
|
fn ser(&self, _: MavlinkVersion, payload: &mut [u8]) -> usize {
|
|
payload[..Self::ENCODED_LEN].copy_from_slice(&[0, 0, 0, 0, 0, 0, 0, 0, 3]);
|
|
Self::ENCODED_LEN
|
|
}
|
|
|
|
fn deser(_: MavlinkVersion, _: &[u8]) -> Result<Self, ParserError> {
|
|
Ok(Self)
|
|
}
|
|
}
|
|
|
|
struct MalformedKaptanHeartbeat;
|
|
|
|
impl MessageData for MalformedKaptanHeartbeat {
|
|
type Message = MavMessage;
|
|
|
|
const ID: u32 = HEARTBEAT_DATA::ID;
|
|
const NAME: &'static str = HEARTBEAT_DATA::NAME;
|
|
const EXTRA_CRC: u8 = HEARTBEAT_DATA::EXTRA_CRC;
|
|
const ENCODED_LEN: usize = HEARTBEAT_DATA::ENCODED_LEN;
|
|
|
|
fn ser(&self, _: MavlinkVersion, payload: &mut [u8]) -> usize {
|
|
payload[0] = 0;
|
|
Self::ENCODED_LEN
|
|
}
|
|
|
|
fn deser(_: MavlinkVersion, _: &[u8]) -> Result<Self, ParserError> {
|
|
Ok(Self)
|
|
}
|
|
}
|
|
|
|
fn heartbeat(sequence: u8) -> MavlinkFrame {
|
|
let mut frame = MavlinkFrame::new();
|
|
frame.serialize_message_data(
|
|
MavHeader {
|
|
sequence,
|
|
system_id: 42,
|
|
component_id: 7,
|
|
},
|
|
&HEARTBEAT_DATA {
|
|
state: HeartbeatState::HEARTBEAT_STATE_INITIALIZED_OUTPUTS_INHIBITED,
|
|
},
|
|
);
|
|
frame
|
|
}
|
|
|
|
fn foreign_heartbeat_v1(sequence: u8) -> MAVLinkV1MessageRaw {
|
|
let mut frame = MAVLinkV1MessageRaw::new();
|
|
frame.serialize_message_data(
|
|
MavHeader {
|
|
sequence,
|
|
system_id: 42,
|
|
component_id: 7,
|
|
},
|
|
&ForeignHeartbeat,
|
|
);
|
|
frame
|
|
}
|
|
|
|
fn foreign_heartbeat_v2(sequence: u8) -> MavlinkFrame {
|
|
let mut frame = MavlinkFrame::new();
|
|
frame.serialize_message_data(
|
|
MavHeader {
|
|
sequence,
|
|
system_id: 42,
|
|
component_id: 7,
|
|
},
|
|
&ForeignHeartbeat,
|
|
);
|
|
frame
|
|
}
|
|
|
|
fn malformed_kaptan_heartbeat(sequence: u8) -> MavlinkFrame {
|
|
let mut frame = MavlinkFrame::new();
|
|
frame.serialize_message_data(
|
|
MavHeader {
|
|
sequence,
|
|
system_id: 42,
|
|
component_id: 7,
|
|
},
|
|
&MalformedKaptanHeartbeat,
|
|
);
|
|
frame
|
|
}
|
|
|
|
#[test]
|
|
fn discards_a_crc_valid_mavlink_v1_frame() {
|
|
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
|
|
let v1 = foreign_heartbeat_v1(3);
|
|
assert_eq!(v1.raw_bytes()[0], 0xfe);
|
|
|
|
for byte in v1.raw_bytes() {
|
|
assert!(!receiver.receive_byte(*byte));
|
|
}
|
|
|
|
assert!(receiver.take_received().is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn discards_a_crc_valid_foreign_dialect_frame() {
|
|
let foreign = foreign_heartbeat_v2(3);
|
|
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
|
|
|
|
for byte in foreign.raw_bytes() {
|
|
assert!(!receiver.receive_byte(*byte));
|
|
}
|
|
|
|
assert!(receiver.take_received().is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn queues_a_complete_crc_valid_unsigned_frame() {
|
|
let frame = heartbeat(9);
|
|
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
|
|
for byte in frame.raw_bytes() {
|
|
receiver.receive_byte(*byte);
|
|
}
|
|
let received = receiver.take_received().expect("complete frame");
|
|
assert_eq!(received.raw(), &frame);
|
|
assert_eq!(received.decode(), crate::decode_message(&frame));
|
|
assert!(receiver.take_received().is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn defers_payload_decoding_until_after_integrity_admission() {
|
|
let frame = malformed_kaptan_heartbeat(9);
|
|
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
|
|
for byte in frame.raw_bytes() {
|
|
receiver.receive_byte(*byte);
|
|
}
|
|
|
|
let received = receiver.take_received().expect("CRC-valid Kaptan frame");
|
|
assert_eq!(received.raw(), &frame);
|
|
assert_eq!(received.decode(), None);
|
|
}
|
|
|
|
#[test]
|
|
fn integrity_wrapper_has_exactly_the_raw_frame_layout() {
|
|
assert_eq!(
|
|
core::mem::size_of::<IntegrityCheckedFrame>(),
|
|
core::mem::size_of::<MavlinkFrame>()
|
|
);
|
|
assert_eq!(
|
|
core::mem::align_of::<IntegrityCheckedFrame>(),
|
|
core::mem::align_of::<MavlinkFrame>()
|
|
);
|
|
}
|
|
|
|
#[test]
|
|
fn rejects_signed_v2_before_accepting_any_payload() {
|
|
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
|
|
// The first ten bytes contain the complete MAVLink 2 header. This
|
|
// frame advertises a signature (incompatibility bit 0), so rejection
|
|
// happens before its payload can be considered.
|
|
for byte in [0xfd, 0, 1, 0, 0, 1, 1, 0, 0, 0] {
|
|
assert!(!receiver.receive_byte(byte));
|
|
}
|
|
assert!(receiver.take_received().is_none());
|
|
}
|
|
|
|
#[test]
|
|
fn a_crc_error_cannot_create_a_received_record() {
|
|
let frame = heartbeat(0);
|
|
let mut bytes = frame.into_inner();
|
|
bytes[10] ^= 1;
|
|
let corrupt = MavlinkFrame::from_bytes_unparsed(bytes);
|
|
let mut receiver = IntegrityMavlinkReceiver::<1>::new();
|
|
|
|
for byte in corrupt.raw_bytes() {
|
|
assert!(!receiver.receive_byte(*byte));
|
|
}
|
|
assert!(receiver.take_received().is_none());
|
|
}
|
|
}
|