From aa14493e54f96917b1bb88de34182533b5edc486 Mon Sep 17 00:00:00 2001 From: Peter Krull Date: Mon, 29 Apr 2024 17:14:13 +0200 Subject: [PATCH] refactor commit 1 --- Cargo.toml | 1 + examples/local_serial.rs | 36 +- src/address.rs | 69 +++ src/buffer.rs | 47 +- src/crc8.rs | 54 ++ src/lib.rs | 794 +++++++++++++++++------------- src/packets/link_statistics.rs | 98 ++-- src/packets/mod.rs | 83 +++- src/packets/rc_channels.rs | 141 ------ src/packets/rc_channels_packed.rs | 110 +++++ src/raw_packet.rs | 96 ++++ 11 files changed, 933 insertions(+), 596 deletions(-) create mode 100644 src/address.rs create mode 100644 src/crc8.rs delete mode 100644 src/packets/rc_channels.rs create mode 100644 src/packets/rc_channels_packed.rs create mode 100644 src/raw_packet.rs diff --git a/Cargo.toml b/Cargo.toml index 5396d16..2cfc2cb 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -9,6 +9,7 @@ repository = "https://github.com/tact1m4n3/crsf-rs" documentation = "https://docs.rs/crsf" [dependencies] +bitflags = "2.5.0" crc = "3.2" defmt = { version = "0.3.6", optional = true } num_enum = { version = "0.7.2", default-features = false } diff --git a/examples/local_serial.rs b/examples/local_serial.rs index 70f9297..490b81f 100644 --- a/examples/local_serial.rs +++ b/examples/local_serial.rs @@ -1,6 +1,6 @@ use std::{env, io, time::Duration}; -use crsf::{Packet, PacketPayload, PacketReader}; +use crsf::{Packet, PacketReader}; fn main() { let path = env::args().nth(1).expect("no serial port supplied"); @@ -13,27 +13,25 @@ fn main() { let mut reader = PacketReader::new(); loop { match port.read(buf.as_mut_slice()) { - Ok(n) => { - if n > 0 { - let mut remaining = &buf[..n]; - while let (Some(raw_packet), consumed) = reader.push_bytes(remaining) { - match Packet::parse(raw_packet) { - Ok(packet) => match packet.payload { - PacketPayload::LinkStatistics(link_statistics) => { - println!("{:?}", link_statistics); - } - PacketPayload::RcChannels(channels) => { - println!("{:?}", channels); - } - _ => {} - }, - Err(err) => eprintln!("{err}"), - }; - - remaining = &remaining[consumed..]; + Ok(n @ 1..) => { + for result in reader.iter_packets(&buf[..n]) { + match result { + Ok(Packet::LinkStatistics(link_stats)) => { + println!("{:?}", link_stats); + } + Ok(Packet::RcChannelsPacked(rc_channels)) => { + println!("{:?}", rc_channels); + } + _ => { + eprintln!("Unknown packet"); + } } } } + Ok(0) => { + eprintln!("EOF"); + break; + } Err(ref e) if e.kind() == io::ErrorKind::TimedOut => (), Err(e) => { eprintln!("{}", e); diff --git a/src/address.rs b/src/address.rs new file mode 100644 index 0000000..9de42d3 --- /dev/null +++ b/src/address.rs @@ -0,0 +1,69 @@ +use num_enum::TryFromPrimitive; + +/// Represents all CRSF packet addresses +#[non_exhaustive] +#[derive(Clone, Copy, Debug, PartialEq, Eq, TryFromPrimitive)] +#[repr(u8)] +pub enum PacketAddress { + Broadcast = 0x00, + Usb = 0x10, + Bluetooth = 0x12, + TbsCorePnpPro = 0x80, + Reserved1 = 0x8A, + CurrentSensor = 0xC0, + Gps = 0xC2, + TbsBlackbox = 0xC4, + FlightController = 0xC8, + Reserved2 = 0xCA, + RaceTag = 0xCC, + Handset = 0xEA, + Receiver = 0xEC, + Transmitter = 0xEE, +} + +bitflags::bitflags! { + pub(crate) struct PacketAddressFlags: u16 { + const BROADCAST = 1; + const USB = 1 << 1; + const BLUETOOTH = 1 << 2; + const TBS_CORE_PNP_PRO = 1 << 3; + const RESERVED1 = 1 << 4; + const CURRENT_SENSOR = 1 << 5; + const GPS = 1 << 6; + const TBS_BLACKBOX = 1 << 7; + const FLIGHT_CONTROLLER = 1 << 8; + const RESERVED2 = 1 << 9; + const RACE_TAG = 1 << 10; + const HANDSET = 1 << 11; + const RECEIVER = 1 << 12; + const TRANSMITTER = 1 << 13; + } +} + +impl PacketAddressFlags { + pub(crate) fn from_address(address: PacketAddress) -> Self { + use PacketAddress::*; + + match address { + Broadcast => PacketAddressFlags::BROADCAST, + Usb => PacketAddressFlags::USB, + Bluetooth => PacketAddressFlags::BLUETOOTH, + TbsCorePnpPro => PacketAddressFlags::TBS_CORE_PNP_PRO, + Reserved1 => PacketAddressFlags::RESERVED1, + CurrentSensor => PacketAddressFlags::CURRENT_SENSOR, + Gps => PacketAddressFlags::GPS, + TbsBlackbox => PacketAddressFlags::TBS_BLACKBOX, + FlightController => PacketAddressFlags::FLIGHT_CONTROLLER, + Reserved2 => PacketAddressFlags::RESERVED2, + RaceTag => PacketAddressFlags::RACE_TAG, + Handset => PacketAddressFlags::HANDSET, + Receiver => PacketAddressFlags::RECEIVER, + Transmitter => PacketAddressFlags::TRANSMITTER, + } + } + + pub(crate) fn contains_u8(&self, value: u8) -> bool { + PacketAddress::try_from(value) + .map_or(false, |address| self.contains(Self::from_address(address))) + } +} diff --git a/src/buffer.rs b/src/buffer.rs index 27e2d19..b285136 100644 --- a/src/buffer.rs +++ b/src/buffer.rs @@ -8,8 +8,10 @@ impl<'a> BytesReader<'a> { Self { buf, idx: 0 } } - pub fn consumed(&self) -> usize { - self.idx + // Get a slice of the remaining part of the buffer + pub fn remaining(&self) -> &'a [u8] { + debug_assert!(self.idx <= self.buf.len()); + self.buf.get(self.idx..).unwrap_or(&[]) } pub fn is_empty(&self) -> bool { @@ -33,44 +35,3 @@ impl<'a> BytesReader<'a> { data } } - -pub(crate) struct Buf { - buf: [u8; C], - len: usize, -} - -impl Buf { - pub const fn new() -> Self { - Self { - buf: [0; C], - len: 0, - } - } - - pub fn len(&self) -> usize { - self.len - } - - pub fn push(&mut self, c: u8) -> bool { - if let Some(v) = self.buf.get_mut(self.len) { - *v = c; - self.len += 1; - true - } else { - false - } - } - - pub fn push_bytes(&mut self, data: &[u8]) { - self.buf[self.len..self.len + data.len()].copy_from_slice(data); - self.len += data.len(); - } - - pub fn data(&self) -> &[u8; C] { - &self.buf - } - - pub fn clear(&mut self) { - self.len = 0; - } -} diff --git a/src/crc8.rs b/src/crc8.rs new file mode 100644 index 0000000..fe29d19 --- /dev/null +++ b/src/crc8.rs @@ -0,0 +1,54 @@ +/// Create a new look-up table for the CRC8 algorithm. +pub const fn new_crc8_lut() -> [u8; 256] { + let mut crc_table = [0u8; 256]; + + let mut i = 0; + while i < 256 { + let mut crc = i as u8; + let mut j = 0; + while j < 8 { + if crc & 0x80 != 0 { + crc = (crc << 1) ^ 0xd5; + } else { + crc <<= 1; + } + j += 1; + } + crc_table[i] = crc; + i += 1; + } + + crc_table +} + +const CRC8_LUT: [u8; 256] = new_crc8_lut(); + +/// Software based CRC8 implementation. +#[derive(Debug)] +pub struct Crc8 { + crc_val: u8, + crc_table: &'static [u8; 256], +} + +impl Crc8 { + pub const fn new() -> Self { + Crc8 { + crc_table: &CRC8_LUT, + crc_val: 0, + } + } + + pub fn compute(&mut self, data: &[u8]) { + for e in data { + self.crc_val = self.crc_table[(self.crc_val ^ e) as usize]; + } + } + + pub fn reset(&mut self) { + self.crc_val = 0; + } + + pub fn get_checksum(&self) -> u8 { + self.crc_val + } +} diff --git a/src/lib.rs b/src/lib.rs index 2a7b371..243eb91 100644 --- a/src/lib.rs +++ b/src/lib.rs @@ -2,41 +2,29 @@ //! # Usage //! ### Packet Parsing //! ```rust -//! use crsf::{Packet, PacketReader, PacketAddress, PacketType}; +//! use crsf::{Packet, PacketReader, PacketAddress, PacketType, RcChannelsPacked}; //! //! let mut reader = PacketReader::new(); //! let data: &[&[u8]] = &[&[0xc8, 24, 0x16], &[0; 22], &[239]]; -//! for input_buf in data { -//! let mut buf: &[u8] = input_buf; -//! while !buf.is_empty() { -//! let consumed = match reader.push_bytes(buf) { -//! (Some(raw_packet), n) => { -//! let packet = Packet::parse(raw_packet).expect("valid packet"); -//! n -//! } -//! (None, n) => n, -//! }; -//! buf = &buf[consumed..]; +//! for (i, input_buf) in data.iter().enumerate() { +//! for (j, result) in reader.iter_packets(input_buf).enumerate() { +//! match result { +//! Ok(Packet::RcChannelsPacked(rc_channels))=> assert_eq!(rc_channels, RcChannelsPacked([0u16; 16])), +//! e => panic!("This data should parse succesfully: {e:?}, {i}, {j}"), +//! } //! } //! } -//! -//! let addr = PacketAddress::FlightController; -//! let typ = PacketType::RcChannelsPacked; //! ``` //! ### Packet Construction //! ```rust -//! use crsf::{Packet, PacketAddress, PacketPayload, RcChannels}; +//! use crsf::{PacketAddress, PacketType, RcChannelsPacked, Payload}; //! //! let channels: [u16; 16] = [0xffff; 16]; -//! let packet = Packet::new( -//! PacketAddress::FlightController, -//! PacketPayload::RcChannels(RcChannels(channels)) -//! ); -//! -//! let mut buf = [0u8; Packet::MAX_LENGTH]; -//! let packet_len = packet.dump(&mut buf).expect("dumped packet"); -//! let packet_data = &buf[..packet_len]; -//! +//! let addr = PacketAddress::FlightController; +//! let payload = RcChannelsPacked(channels); +//! +//! // Import the `Payload` trait to construct a raw packet +//! let raw_packet = payload.into_raw_packet_with_sync(addr as u8).unwrap(); //! // ... //! ``` @@ -44,40 +32,135 @@ // TODO: top level crate packet reader examples redo -use crc::{Crc, CRC_8_DVB_S2}; +pub use address::PacketAddress; +use address::PacketAddressFlags; #[cfg(feature = "defmt")] use defmt; use num_enum::TryFromPrimitive; use snafu::prelude::*; -/// Used to calculate the CRC8 checksum -#[link_section = ".data"] -static CRC8: Crc = Crc::::new(&CRC_8_DVB_S2); - -use buffer::{Buf, BytesReader}; -pub use packets::*; +mod address; +mod to_array; mod buffer; +use buffer::BytesReader; +mod crc8; +use crc8::Crc8; + +mod raw_packet; +pub use raw_packet::*; mod packets; -mod to_array; +pub use packets::*; + +pub const CRSF_MAX_LEN: usize = 64; +const CRSF_HEADER_LEN: usize = 2; +const CRSF_SYNC_BYTE: u8 = 0xC8; + +/// Represents a state machine for reading a CRSF packet +/// +/// +--------------+ +-------------+ +---------+ +/// | AwaitingSync |-->| AwaitingLen |-->| Reading | +/// +--------------+ +-------------+ +---------+ +/// ^ | | +/// | | | +/// +-------------------+ | +/// +------------------------------------+ +/// +enum ReadState { + AwaitingSync, + AwaitingLen, + Reading, +} /// Represents a packet reader pub struct PacketReader { - buf: Buf<{ Packet::MAX_LENGTH }>, state: ReadState, + raw: RawPacket, + digest: Crc8, + config: Config, +} + +impl Default for PacketReader { + /// Creates a new PacketReader with the default configuration + fn default() -> Self { + Self { + state: ReadState::AwaitingSync, + raw: RawPacket::empty(), + digest: Crc8::new(), + config: Config::default(), + } + } +} + +pub struct PacketReaderBuilder { + config: Config, +} + +impl PacketReaderBuilder { + pub fn sync(mut self, sync: &[PacketAddress]) -> Self { + let mut sync_byte = PacketAddressFlags::empty(); + for addr in sync { + sync_byte |= PacketAddressFlags::from_address(*addr); + } + self.config.sync = sync_byte; + self + } + + pub fn type_check(mut self, type_check: bool) -> Self { + self.config.type_check = type_check; + self + } + + pub fn build(self) -> PacketReader { + PacketReader { + state: ReadState::AwaitingSync, + raw: RawPacket::empty(), + digest: Crc8::new(), + config: self.config, + } + } +} + +pub struct Config { + /// Sync byte to use for finding the start of a frame. Default is `0xC8` + sync: PacketAddressFlags, + + /// Whether to ensure the type byte is a valid PacketType enum value. Default is `true`. + type_check: bool, +} + +impl Config { + // Not using default trait because it is not const + const fn default() -> Self { + Self { + sync: PacketAddressFlags::FLIGHT_CONTROLLER, + type_check: true, + } + } } impl PacketReader { - // Packet type and checksum bytes are mandatory - const MIN_DATA_LENGTH: u8 = 2; - // Number of bytes of packet type, payload and checksum - const MAX_DATA_LENGTH: u8 = Packet::MAX_LENGTH as u8 - Self::MIN_DATA_LENGTH; + // Minimum data length, must include type and crc bytes + const MIN_LEN_BYTE: u8 = 2; + // Maximum data length, includes type, payload and crc bytes + const MAX_LEN_BYTE: u8 = CRSF_MAX_LEN as u8 - Self::MIN_LEN_BYTE; /// Creates a new PacketReader struct pub const fn new() -> Self { Self { - buf: Buf::new(), - state: ReadState::WaitingForSync, + state: ReadState::AwaitingSync, + raw: RawPacket::empty(), + digest: Crc8::new(), + config: Config::default(), + } + } + + pub const fn builder() -> PacketReaderBuilder { + PacketReaderBuilder { + config: Config { + sync: PacketAddressFlags::FLIGHT_CONTROLLER, + type_check: true, + }, } } @@ -85,238 +168,192 @@ impl PacketReader { /// /// Useful in situations when timeout is triggered but a packet is not parsed pub fn reset(&mut self) { - self.buf.clear(); - self.state = ReadState::WaitingForSync; + self.state = ReadState::AwaitingSync; + self.raw.len = 0; // Soft-reset the buffer + self.digest.reset(); } /// Reads the first packet from the buffer - pub fn push_bytes(&mut self, bytes: &[u8]) -> (Option, usize) { + pub fn push_bytes<'r, 'b>( + &'r mut self, + bytes: &'b [u8], + ) -> (Option>, &'b [u8]) { let mut reader = BytesReader::new(bytes); - let packet = loop { + let packet = 'state_machine: loop { match self.state { - ReadState::WaitingForSync => { - while let Some(addr_byte) = reader.next() { - if let Ok(addr) = PacketAddress::try_from(addr_byte) { - self.buf.clear(); - self.buf.push(addr_byte); - self.state = ReadState::WaitingForLen { addr }; - break; + ReadState::AwaitingSync => { + while let Some(sync_byte) = reader.next() { + if self.config.sync.contains_u8(sync_byte) { + self.raw.buf[0] = sync_byte; + self.state = ReadState::AwaitingLen; + continue 'state_machine; } } + if reader.is_empty() { - break None; - } else { - continue; + break Some(Err(CrsfError::NoSyncByte)); } } - ReadState::WaitingForLen { addr } => { - if let Some(len_byte) = reader.next() { - match len_byte { - Self::MIN_DATA_LENGTH..=Self::MAX_DATA_LENGTH => { - self.buf.push(len_byte); - self.state = ReadState::Reading { - addr, - len: Packet::HEADER_LENGTH + len_byte as usize, - }; - } - _ => self.state = ReadState::WaitingForSync, + ReadState::AwaitingLen => { + let Some(len_byte) = reader.next() else { + break None; + }; + match len_byte { + Self::MIN_LEN_BYTE..=Self::MAX_LEN_BYTE => { + self.raw.buf[1] = len_byte; + self.raw.len = CRSF_HEADER_LEN; + self.state = ReadState::Reading; + } + _ => { + self.reset(); + break Some(Err(CrsfError::InvalidLength { len: len_byte })); } - continue; } } - ReadState::Reading { addr, len } => { - let data = reader.next_n(len - self.buf.len()); - self.buf.push_bytes(data); - if self.buf.len() >= len { - self.state = ReadState::WaitingForSync; - break Some( - RawPacket { - addr, - buf: self.buf.data(), - len: self.buf.len(), - } - ); + ReadState::Reading => { + if reader.is_empty() { + break None; + } + + let final_len = self.raw.buf[1] as usize + CRSF_HEADER_LEN; + let data = reader.next_n(final_len - self.raw.len); + self.raw.buf[self.raw.len..self.raw.len + data.len()].copy_from_slice(data); + self.raw.len += data.len(); + + // Validate that type is in PacketType enum + if let Some(type_byte) = self.raw.buf.get(2).copied() { + if self.config.type_check && PacketType::try_from(type_byte).is_err() + { + self.reset(); + break Some(Err(CrsfError::UnknownType { + typ: type_byte, + })); + } + } + + // If we have received the CRC byte, do not use it in the digest + if self.raw.len == final_len { + self.digest.compute(&data[..data.len() - 1]); + let act_crc = self.digest.get_checksum(); + let exp_crc = self.raw.buf[self.raw.len - 1]; + if act_crc != exp_crc { + self.reset(); + break Some(Err(CrsfError::CrcMismatch { + exp: exp_crc, + act: act_crc, + })); + } + } else { + self.digest.compute(data); + } + + if self.raw.len >= final_len { + self.digest.reset(); + self.state = ReadState::AwaitingSync; + break Some(Ok(&self.raw)); } } } - - break None; }; - (packet, reader.consumed()) + (packet, reader.remaining()) } -} - -impl Default for PacketReader { - fn default() -> Self { - Self::new() - } -} -/// Represents a raw packet (not parsed) -#[derive(Clone, Copy, Debug)] -pub struct RawPacket<'a> { - addr: PacketAddress, - buf: &'a [u8; Packet::MAX_LENGTH], - len: usize, -} - -impl<'a> RawPacket<'a> { - /// Returns the packet data as a slice - pub fn as_slice(&self) -> &[u8] { - &self.buf[..self.len] + /// Returns an interator over the given buffer. If the buffer contains packet of a valid format, + /// the iterator will return `Ok(RawPacket)`. If the buffer contains invalid packets, the iterator + /// will return `Err(CrsfError)`. If the buffer is too small to parse, the iterator will yield. + /// Once the iterator yields, all bytes in the buffer have been consumed. + /// + /// To get an iterator that returns `Packet`, use `iter_packets`. + pub fn iter_raw_packets<'a, 'b>(&'a mut self, buf: &'b [u8]) -> IterRawPackets<'a, 'b> { + IterRawPackets { parser: self, buf } } - pub fn addr(&self) -> PacketAddress { - self.addr + /// Returns an iterator over the given buffer. If the buffer contains packets of a valid format, + /// the iterator will return `Ok(Packet)`. If the buffer contains invalid packets, the iterator + /// will return `Err(CrsfError)`. If the buffer is too small to parse, the iterator will yield. + /// Once the iterator yields, all bytes in the buffer have been consumed. + /// + /// To get an iterator that returns `RawPacket`, use `iter_raw_packets`. + pub fn iter_packets<'a, 'b>(&'a mut self, buf: &'b [u8]) -> IterPackets<'a, 'b> { + IterPackets { parser: self, buf } } } -/// Represents a packet payload data -#[non_exhaustive] -#[derive(Clone, Debug)] -pub enum PacketPayload { - LinkStatistics(LinkStatistics), - RcChannels(RcChannels), -} -/// Represents a parsed packet -pub struct Packet { - pub addr: PacketAddress, - pub payload: PacketPayload, +/// An iterator over a buffer that yield `RawPacket` instances, or `CrsfError` in case of currupt data. +/// This iterator will consume the and process the entire buffer. For an iterator that also parses the +/// packets into `Packet` instances, use `IterPackets` instead. +pub struct IterRawPackets<'a, 'b> { + parser: &'a mut PacketReader, + buf: &'b [u8], } -impl Packet { - /// Max crsf packet length - pub const MAX_LENGTH: usize = 64; - /// Crsf packet header length - pub const HEADER_LENGTH: usize = 2; +impl<'a, 'b> Iterator for IterRawPackets<'a, 'b> { + type Item = Result; - /// Creates new packet with address and payload - pub fn new(addr: PacketAddress, payload: PacketPayload) -> Self { - Self { addr, payload } - } - - /// Parses a raw packet (returned by the packet reader) - pub fn parse(raw_packet: RawPacket) -> Result { - // TODO: use more constants instead of literals - - // not using raw_packet.as_slice() for the compiler to remove out of bounds checking - let buf = raw_packet.buf; - let len = raw_packet.len; - - let checksum_idx = len - 1; - let checksum = CRC8.checksum(&buf[2..checksum_idx]); - if checksum != buf[checksum_idx] { - return Err(ParseError::ChecksumMismatch { - expected: checksum, - actual: buf[checksum_idx], - }); - } - - let typ_byte = buf[2]; - if let Ok(typ) = PacketType::try_from(typ_byte) { - let payload_data = if typ.is_extended() { - &buf[5..len] - } else { - &buf[3..len] - }; - - let payload = match typ { - PacketType::RcChannelsPacked => { - PacketPayload::RcChannels(RcChannels::parse(payload_data)) - } - PacketType::LinkStatistics => { - PacketPayload::LinkStatistics(LinkStatistics::parse(payload_data)) - } - _ => return Err(ParseError::UnknownType { typ: typ_byte }), - }; - Ok(Packet { - addr: raw_packet.addr, - payload, - }) - } else { - Err(ParseError::InvalidType { typ: typ_byte }) + fn next(&mut self) -> Option { + if self.buf.is_empty() { + return None; } + let result; + (result, self.buf) = self.parser.push_bytes(self.buf); + result.map(|raw| raw.cloned()) } +} - /// Dumps the packet into a buffer - pub fn dump(&self, buf: &mut [u8]) -> Result { - // TODO: use more constants instead of literals - - let payload = match &self.payload { - PacketPayload::LinkStatistics(payload) => payload as &dyn Payload, - PacketPayload::RcChannels(payload) => payload as &dyn Payload, - }; +/// An iterator over a buffer that return parsed `Packet` instances, or `CrsfError` in case of currupt data. +/// This iterator will consume the and process the entire buffer. +pub struct IterPackets<'a, 'b> { + parser: &'a mut PacketReader, + buf: &'b [u8], +} - let typ = payload.packet_type(); - let len_byte = payload.len() + if typ.is_extended() { 4 } else { 2 }; - let len = Packet::HEADER_LENGTH + len_byte as usize; +impl<'a, 'b> Iterator for IterPackets<'a, 'b> { + type Item = Result; - if buf.len() < len { - return Err(BufferLenError { - expected: len, - actual: buf.len(), - }); + fn next(&mut self) -> Option { + if self.buf.is_empty() { + return None; } - - buf[0] = self.addr as u8; - buf[1] = len_byte; - buf[2] = typ as u8; - - let payload_start = if typ.is_extended() { 5 } else { 3 }; - let checksum_idx = len - 1; - payload.dump(&mut buf[payload_start..checksum_idx]); - buf[checksum_idx] = CRC8.checksum(&buf[2..checksum_idx]); - - Ok(len) + let result; + (result, self.buf) = self.parser.push_bytes(self.buf); + result.map(|res| match res { + Ok(raw) => raw.into_packet(), + Err(err) => Err(err), + }) } } + /// Represents packet parsing errors #[non_exhaustive] #[derive(Debug, PartialEq, Snafu)] #[cfg_attr(feature = "defmt", derive(defmt::Format))] -pub enum ParseError { - #[snafu(display("Unknown type: {typ:#04x}"))] +pub enum CrsfError { + #[snafu(display("No sync byte was found in the given buffer"))] + NoSyncByte, + #[snafu(display("Unknown type: {typ:#04x}, see PacketType enum for valid types"))] UnknownType { typ: u8 }, - #[snafu(display("Invalid type: {typ:#04x}"))] - InvalidType { typ: u8 }, - #[snafu(display("Checksum mismatch: expected {expected:#04x}, but got {actual:#04x}"))] - ChecksumMismatch { expected: u8, actual: u8 }, + #[snafu(display("Invalid length: {len}, should be between 2 and 62"))] + InvalidLength { len: u8 }, + #[snafu(display("Crc checksum mismatch: expected {exp:#04x}, got {act:#04x}"))] + CrcMismatch { exp: u8, act: u8 }, + #[snafu(display("A general buffer error relating to the parser occured"))] + BufferError, + #[snafu(display("Invalid payload data, could not parse packet"))] + InvalidPayload, } -/// Represents a buffer too small error -#[derive(Debug, PartialEq, Snafu)] -#[snafu(display( - "Dump buffer too small: expected len of at least {expected} bytes, but got {actual} bytes" -))] -pub struct BufferLenError { - expected: usize, - actual: usize, -} - -/// Represents all CRSF packet addresses +/// Represents a packet payload data #[non_exhaustive] -#[derive(Clone, Copy, Debug, PartialEq, Eq, TryFromPrimitive)] -#[repr(u8)] -pub enum PacketAddress { - Broadcast = 0x00, - Usb = 0x10, - Bluetooth = 0x12, - TbsCorePnpPro = 0x80, - Reserved1 = 0x8A, - CurrentSensor = 0xC0, - Gps = 0xC2, - TbsBlackbox = 0xC4, - FlightController = 0xC8, - Reserved2 = 0xCA, - RaceTag = 0xCC, - Handset = 0xEA, - Receiver = 0xEC, - Transmitter = 0xEE, +#[derive(Clone, Debug, PartialEq)] +pub enum Packet { + LinkStatistics(LinkStatistics), + RcChannelsPacked(RcChannelsPacked), } + /// Represents all CRSF packet types #[non_exhaustive] #[derive(Clone, Copy, Debug, PartialEq, Eq, TryFromPrimitive)] @@ -356,73 +393,70 @@ impl PacketType { } } -/// Represents a state machine for reading a CRSF packet -/// -/// +---------------+ +---------------+ +---------+ -/// | WatingForSync |-->| WaitingForLen |-->| Reading | -/// +---------------+ +---------------+ +---------+ -/// ^ | | -/// | | | -/// +-------------------+ | -/// +------------------------------------+ -/// -enum ReadState { - WaitingForSync, - WaitingForLen { addr: PacketAddress }, - Reading { addr: PacketAddress, len: usize }, -} - #[cfg(test)] mod tests { - use crate::BufferLenError; + use crate::address::PacketAddress; use crate::BytesReader; + use crate::CrsfError; use crate::LinkStatistics; use crate::Packet; - use crate::PacketAddress; - use crate::PacketPayload; use crate::PacketReader; use crate::PacketType; - use crate::ParseError; - use crate::RcChannels; + use crate::Payload; + use crate::RcChannelsPacked; #[test] fn test_bytes_reader() { let bytes: &[u8] = &[1, 2, 3, 4, 5]; let mut reader = BytesReader::new(bytes); assert_eq!(reader.next(), Some(1)); - assert_eq!(reader.consumed(), 1); + assert_eq!(reader.remaining(), &[2, 3, 4, 5]); assert_eq!(reader.next_n(2), &[2, 3]); + assert_eq!(reader.remaining(), &[4, 5]); assert_eq!(reader.next(), Some(4)); assert_eq!(reader.next(), Some(5)); - assert_eq!(reader.consumed(), 5); + assert_eq!(reader.remaining(), &[]); } #[test] fn test_packet_reader_waiting_for_sync_byte() { - let mut reader = PacketReader::new(); - let typ = PacketType::RcChannelsPacked; + let addr = PacketAddress::Handset; + let mut reader = PacketReader::builder().sync(&[addr]).build(); + + let typ = PacketType::RcChannelsPacked as u8; for _ in 0..2 { // Garbage - assert!(reader.push_bytes(&[1, 2, 3]).0.is_none()); + assert!(matches!( + reader.push_bytes(&[1, 2, 3]).0, + Some(Err(CrsfError::NoSyncByte)) + )); // More garbage - assert!(reader.push_bytes(&[254, 255]).0.is_none()); + assert!(matches!( + reader.push_bytes(&[254, 255]).0, + Some(Err(CrsfError::NoSyncByte)) + )); // Sync - assert!(reader.push_bytes(&[PacketAddress::Handset as u8]).0.is_none()); + assert!(reader.push_bytes(&[addr as u8]).0.is_none()); // Len assert!(reader.push_bytes(&[24]).0.is_none()); // Type - assert!(reader.push_bytes(&[typ as u8]).0.is_none()); + assert!(reader.push_bytes(&[typ]).0.is_none()); // Payload assert!(reader.push_bytes(&[0; 22]).0.is_none()); + // Checksum - assert!(matches!( - reader.push_bytes(&[239]).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), - Ok(Packet { - addr: PacketAddress::Handset, - payload: PacketPayload::RcChannels(RcChannels(channels)) - }) if channels == [0; 16] - )); + let result = reader.push_bytes(&[239]).0.expect("result expected"); + + let raw_packet = result.expect("raw packet expected"); + let packet = raw_packet.into_packet().expect("packet expected"); + + match packet { + Packet::RcChannelsPacked(ch) => { + ch.0.iter().all(|&x| x == 0); + } + _ => panic!("unexpected packet type"), + } } } @@ -442,13 +476,83 @@ mod tests { // Payload assert!(reader.push_bytes(&[0; 22]).0.is_none()); // Checksum - assert!(matches!( - reader.push_bytes(&[239]).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), - Ok(Packet { - addr: PacketAddress::FlightController, - payload: PacketPayload::RcChannels(RcChannels(channels)) - }) if channels == [0; 16] - )); + let result = reader.push_bytes(&[239]).0.expect("result expected"); + + let raw_packet = result.expect("raw packet expected"); + let packet = raw_packet.into_packet().expect("packet expected"); + + match packet { + Packet::RcChannelsPacked(ch) => { + ch.0.iter().all(|&x| x == 0); + } + _ => panic!("unexpected packet type"), + } + } + + + #[test] + fn test_push_segments() { // similar to the doc-test at the top + + let mut reader = PacketReader::new(); + let data: &[&[u8]] = &[&[0xc8, 24, 0x16], &[0; 22], &[239]]; + for (i, input_buf) in data.iter().enumerate() { + for (j, result) in reader.iter_packets(input_buf).enumerate() { + match result { + Ok(Packet::RcChannelsPacked(rc_channels))=> assert_eq!(rc_channels, RcChannelsPacked([0u16; 16])), + e => panic!("This data should parse succesfully: {e:?}, {i}, {j}"), + } + } + } + } + + + #[test] + fn test_multiple_sync() { + let mut reader = PacketReader::builder() + .sync(&[PacketAddress::FlightController, PacketAddress::Broadcast]) + .build(); + + let rc_channels1 = RcChannelsPacked([1000; 16]); + let raw_packet1 = rc_channels1 + .into_raw_packet_with_sync(PacketAddress::FlightController as u8) + .unwrap(); + + let rc_channels2 = RcChannelsPacked([1500; 16]); + let raw_packet2 = rc_channels2 + .into_raw_packet_with_sync(PacketAddress::Broadcast as u8) + .unwrap(); + + let rc_channels3 = RcChannelsPacked([2000; 16]); // Some other address here ---v + let raw_packet3 = rc_channels3 + .into_raw_packet_with_sync(PacketAddress::Reserved1 as u8) + .unwrap(); + + let result1 = reader + .push_bytes(raw_packet1.as_slice()) + .0 + .expect("result expected") + .expect("raw packet expected"); + assert_eq!( + result1.into_packet().expect("packet expected"), + Packet::RcChannelsPacked(rc_channels1) + ); + + let result2 = reader + .push_bytes(raw_packet2.as_slice()) + .0 + .expect("result expected") + .expect("raw packet expected"); + assert_eq!( + result2.into_packet().expect("packet expected"), + Packet::RcChannelsPacked(rc_channels2) + ); + + let result3 = reader + .push_bytes(raw_packet3.as_slice()) + .0 + .expect("result expected") + .expect_err("Error expected"); + assert!(matches!(result3, CrsfError::NoSyncByte)); } #[test] @@ -460,23 +564,27 @@ mod tests { let data = [ // Sync - addr as u8, - // Len - 24, - // Type - typ as u8, - // Payload - 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, - // Checksum + addr as u8, // Len + 24, // Type + typ as u8, // Payload + 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, // Checksum 239, ]; - assert!(matches!( - reader.push_bytes(&data).0.map(|raw_packet| Packet::parse(raw_packet)).expect("packet expected"), - Ok(Packet { - addr: PacketAddress::FlightController, - payload: PacketPayload::RcChannels(RcChannels(channels)) - }) if channels == [0; 16] - )); + + let result = reader + .push_bytes(data.as_slice()) + .0 + .expect("result expected"); + + let raw_packet = result.expect("raw packet expected"); + let packet = raw_packet.into_packet().expect("packet expected"); + + match packet { + Packet::RcChannelsPacked(ch) => { + ch.0.iter().all(|&x| x == 0); + } + _ => panic!("unexpected packet type"), + } } #[test] @@ -486,94 +594,86 @@ mod tests { let addr = PacketAddress::FlightController; // Sync - reader.push_bytes(&[addr as u8]); + assert!(reader.push_bytes(&[addr as u8]).0.is_none()); // Len - reader.push_bytes(&[24]); + assert!(reader.push_bytes(&[24]).0.is_none()); // Type - reader.push_bytes(&[PacketType::RcChannelsPacked as u8]); + assert!(reader + .push_bytes(&[PacketType::RcChannelsPacked as u8]) + .0 + .is_none()); // Payload - reader.push_bytes(&[0; 22]); + assert!(reader.push_bytes(&[0; 22]).0.is_none()); // Checksum + let result = reader.push_bytes(&[42]).0.expect("result expected"); + assert!(matches!( - reader - .push_bytes(&[42]) - .0 - .map(|raw_packet| Packet::parse(raw_packet)) - .expect("packet error expected"), - Err(ParseError::ChecksumMismatch { - expected: 239, - actual: 42, - }), + result, + Err(CrsfError::CrcMismatch { act: 239, exp: 42 }) )); } - #[test] - fn test_packet_dump_in_small_buffer() { - let packet = Packet::new( - PacketAddress::FlightController, - PacketPayload::LinkStatistics(LinkStatistics { - uplink_rssi_1: 16, - uplink_rssi_2: 19, - uplink_link_quality: 99, - uplink_snr: -105, - active_antenna: 1, - rf_mode: 2, - uplink_tx_power: 3, - downlink_rssi: 8, - downlink_link_quality: 88, - downlink_snr: -108, - }) - ); - - let mut buf = [0u8; 10]; - assert_eq!( - packet.dump(&mut buf), - Err(BufferLenError { - expected: 14, - actual: 10 - }) - ); - } + // This test does not make much sense since there is no "as_raw_packet_into_buf" method. Do we need that? + // #[test] + // fn test_packet_dump_in_small_buffer() { + // let packet = LinkStatistics { + // uplink_rssi_1: 16, + // uplink_rssi_2: 19, + // uplink_link_quality: 99, + // uplink_snr: -105, + // active_antenna: 1, + // rf_mode: 2, + // uplink_tx_power: 3, + // downlink_rssi: 8, + // downlink_link_quality: 88, + // downlink_snr: -108, + // }; + // let mut buf = [0u8; 10]; + // assert_eq!( + // packet.dump(&mut buf), + // Err(BufferLenError { + // expected: 14, + // actual: 10 + // }) + // ); + // } #[test] fn test_rc_channels_packet_dump() { let channels: [u16; 16] = [0x7FF; 16]; - let packet = Packet::new( - PacketAddress::Transmitter, - PacketPayload::RcChannels(RcChannels(channels)) - ); + let addr = PacketAddress::Transmitter; + let packet = RcChannelsPacked(channels); + + let raw = packet.into_raw_packet_with_sync(addr as u8).unwrap(); - let mut buf = [0u8; Packet::MAX_LENGTH]; - let len = packet.dump(&mut buf).unwrap(); let mut expected_data: [u8; 26] = [0xff; 26]; expected_data[0] = 0xee; expected_data[1] = 24; expected_data[2] = 0x16; expected_data[25] = 143; - assert_eq!(&buf[..len], &expected_data) + assert_eq!(&raw.as_slice(), &expected_data) } #[test] fn test_link_statistics_packet_dump() { - let packet = Packet::new( - PacketAddress::FlightController, - PacketPayload::LinkStatistics(LinkStatistics { - uplink_rssi_1: 16, - uplink_rssi_2: 19, - uplink_link_quality: 99, - uplink_snr: -105, - active_antenna: 1, - rf_mode: 2, - uplink_tx_power: 3, - downlink_rssi: 8, - downlink_link_quality: 88, - downlink_snr: -108, - }) - ); + let addr = PacketAddress::FlightController; + + let packet = LinkStatistics { + uplink_rssi_1: 16, + uplink_rssi_2: 19, + uplink_link_quality: 99, + uplink_snr: -105, + active_antenna: 1, + rf_mode: 2, + uplink_tx_power: 3, + downlink_rssi: 8, + downlink_link_quality: 88, + downlink_snr: -108, + }; + + let raw = packet.into_raw_packet_with_sync(addr as u8).unwrap(); - let mut buf = [0u8; Packet::MAX_LENGTH]; - let len = packet.dump(&mut buf).unwrap(); let expected_data = [0xc8, 12, 0x14, 16, 19, 99, 151, 1, 2, 3, 8, 88, 148, 252]; - assert_eq!(&buf[..len], &expected_data) + assert_eq!(raw.as_slice(), expected_data.as_slice()) } } diff --git a/src/packets/link_statistics.rs b/src/packets/link_statistics.rs index 8f7d8f1..5327f0b 100644 --- a/src/packets/link_statistics.rs +++ b/src/packets/link_statistics.rs @@ -1,7 +1,12 @@ -use crate::{Payload, PacketType}; +//! LinkStatistics packet and related functions/implementations + +use super::Payload; +use crate::to_array::{mut_array_start, ref_array_start}; +use crate::{CrsfError, PacketType}; /// Represents a LinkStatistics packet -#[derive(Clone, Debug)] +#[derive(Clone, Debug, PartialEq)] +#[allow(missing_docs)] pub struct LinkStatistics { pub uplink_rssi_1: u8, pub uplink_rssi_2: u8, @@ -15,48 +20,64 @@ pub struct LinkStatistics { pub downlink_snr: i8, } -impl LinkStatistics { - pub(crate) const PAYLOAD_LENGTH: u8 = 10; - - pub(crate) fn parse(data: &[u8]) -> Self { - Self { - uplink_rssi_1: data[0], - uplink_rssi_2: data[1], - uplink_link_quality: data[2], - uplink_snr: data[3] as i8, - active_antenna: data[4], - rf_mode: data[5], - uplink_tx_power: data[6], - downlink_rssi: data[7], - downlink_link_quality: data[8], - downlink_snr: data[9] as i8, - } +const LEN: usize = LinkStatistics::LEN; + +/// The raw decoder (parser) for the LinkStatistics packet. +pub fn raw_decode(data: &[u8; LEN]) -> LinkStatistics { + LinkStatistics { + uplink_rssi_1: data[0], + uplink_rssi_2: data[1], + uplink_link_quality: data[2], + uplink_snr: data[3] as i8, + active_antenna: data[4], + rf_mode: data[5], + uplink_tx_power: data[6], + downlink_rssi: data[7], + downlink_link_quality: data[8], + downlink_snr: data[9] as i8, } } +/// The raw encoder (serializer) for the LinkStatistics packet. +pub fn raw_encode(link_statistics: &LinkStatistics, data: &mut [u8; LEN]) { + data[0] = link_statistics.uplink_rssi_1; + data[1] = link_statistics.uplink_rssi_2; + data[2] = link_statistics.uplink_link_quality; + data[3] = link_statistics.uplink_snr as u8; + data[4] = link_statistics.active_antenna; + data[5] = link_statistics.rf_mode; + data[6] = link_statistics.uplink_tx_power; + data[7] = link_statistics.downlink_rssi; + data[8] = link_statistics.downlink_link_quality; + data[9] = link_statistics.downlink_snr as u8; +} + impl Payload for LinkStatistics { - fn len(&self) -> u8 { Self::PAYLOAD_LENGTH } - - fn packet_type(&self) -> PacketType { PacketType::LinkStatistics } - - fn dump(&self, data: &mut [u8]) { - data[0] = self.uplink_rssi_1; - data[1] = self.uplink_rssi_2; - data[2] = self.uplink_link_quality; - data[3] = self.uplink_snr as u8; - data[4] = self.active_antenna; - data[5] = self.rf_mode; - data[6] = self.uplink_tx_power; - data[7] = self.downlink_rssi; - data[8] = self.downlink_link_quality; - data[9] = self.downlink_snr as u8; + const LEN: usize = 10; + + fn packet_type(&self) -> PacketType { + PacketType::LinkStatistics + } + + fn decode(buf: &[u8]) -> Result { + let data: &[u8; LEN] = ref_array_start(buf).ok_or(CrsfError::BufferError)?; + + Ok(raw_decode(data)) + } + + fn encode<'a>(&self, buf: &'a mut [u8]) -> Result<&'a [u8], crate::CrsfError> { + let data: &mut [u8; LEN] = mut_array_start(buf).ok_or(CrsfError::BufferError)?; + + raw_encode(self, data); + + Ok(data) } } #[cfg(test)] mod tests { - use crate::{LinkStatistics, Packet}; - use crate::packets::Payload; + use super::LinkStatistics; + use crate::Payload; #[test] fn test_link_statistics_write_and_parse() { @@ -73,8 +94,9 @@ mod tests { downlink_snr: -68, }; - let mut data = [0u8; Packet::MAX_LENGTH]; - original.dump(&mut data); + let raw = original.into_raw_packet().unwrap(); + + let data = raw.payload().unwrap(); assert_eq!(data[0], 100_u8.to_le()); assert_eq!(data[1], 98_u8.to_le()); @@ -87,7 +109,7 @@ mod tests { assert_eq!(data[8], 98_u8.to_le()); assert_eq!(data[9], -(68_i8.to_le()) as u8); - let parsed = LinkStatistics::parse(&data); + let parsed = LinkStatistics::decode(&data).unwrap(); assert_eq!(parsed.uplink_rssi_1, 100); assert_eq!(parsed.uplink_rssi_2, 98); diff --git a/src/packets/mod.rs b/src/packets/mod.rs index 6769936..830037e 100644 --- a/src/packets/mod.rs +++ b/src/packets/mod.rs @@ -1,15 +1,82 @@ -pub use link_statistics::*; -pub use rc_channels::*; +//! This module contains defines the behavior of a Payload, and provides implementations for +//! various payloads used in the CRSF protocol. -use crate::PacketType; +use crate::{crc8::Crc8, CrsfError, PacketType, RawPacket, CRSF_MAX_LEN, CRSF_SYNC_BYTE}; -mod link_statistics; -mod rc_channels; +pub mod rc_channels_packed; +pub use rc_channels_packed::RcChannelsPacked; -pub(crate) trait Payload { - fn len(&self) -> u8; +pub mod link_statistics; +pub use link_statistics::LinkStatistics; +/// A trait encapsulationg a CRSF payload. This trait is used to encode and decode payloads +/// to and from byte slices, as well as convert into a [`RawPacket`]s for transmitting elsewhere. +pub trait Payload +where + Self: Sized, +{ + /// The length in bytes of this payload when serialized. + const LEN: usize; + + /// Get the length in bytes of this payload when serialized. + fn len(&self) -> usize { + Self::LEN + } + + /// Get the packet type of this payload. fn packet_type(&self) -> PacketType; - fn dump(&self, buf: &mut [u8]); + /// Decode a payload from a slice. This must not include the `sync`, `len`, `type`, or `crc` bytes. + fn decode(buf: &[u8]) -> Result; + + /// Encode a payload into a mutable slice. This does not include the `sync`, `len`, `type`, or `crc` bytes. + fn encode<'a>(&self, buf: &'a mut [u8]) -> Result<&'a [u8], CrsfError>; + + /// Construct a new `RawPacket` from a `Packet`. This adds the `sync`, `len`, `type` bytes, + /// and calculates and adds the `crc` byte. This constructor assumes the given packet is valid. + fn into_raw_packet(&self) -> Result { + self.into_raw_packet_with_sync(CRSF_SYNC_BYTE) + } + + /// Construct a new `RawPacket` from a `Packet`. This adds the given `sync` byte, `len`, `type` bytes, + /// and calculates and adds the `crc` byte. This constructor assumes the given packet is valid. + /// Note that changing the sync byte is not officially supported by the CRSF protocol, but is used + /// in some implementations as an "address" byte. + fn into_raw_packet_with_sync(&self, sync_byte: u8) -> Result { + let mut raw = RawPacket { + buf: [0u8; CRSF_MAX_LEN], + len: 4 + Self::LEN, + }; + // Insert the payload into the packet + if let Some(payload_buffer) = raw.buf.get_mut(3..) { + self.encode(payload_buffer)?; + } else { + debug_assert!(false, "Failed to get payload buffer") + } + + // Doing this after the encode ensures we do not change + // the contents of the RawPacket if the payload encoding fails. + raw.buf[0] = sync_byte; + raw.buf[1] = 2 + Self::LEN as u8; + raw.buf[2] = self.packet_type() as u8; + + // Calculate the CRC checksum + let mut crc = Crc8::new(); + if let Some(crc_bytes) = raw.buf.get(2..3 + Self::LEN) { + crc.compute(crc_bytes); + } else { + debug_assert!(false, "Failed to get crc bytes") + } + + // Insert the calculated CRC into the packet + if let Some(crc_idx) = raw.buf.get_mut(3 + Self::LEN) { + *crc_idx = crc.get_checksum(); + } else { + debug_assert!(false, "Failed to get crc byte") + } + + raw.len = 4 + Self::LEN; + + Ok(raw) + } } diff --git a/src/packets/rc_channels.rs b/src/packets/rc_channels.rs deleted file mode 100644 index ba928aa..0000000 --- a/src/packets/rc_channels.rs +++ /dev/null @@ -1,141 +0,0 @@ -use crate::{ - to_array::{mut_array_start, ref_array_start}, - PacketType, Payload, -}; - -/// Represents a RcChannelsPacked packet -#[derive(Clone, Debug)] -pub struct RcChannels(pub [u16; 16]); - -impl RcChannels { - /// Minimum channel value - pub const CHANNEL_VALUE_MIN: u16 = 172; - /// Channel value coresponding to 1000 in betaflight - pub const CHANNEL_VALUE_1000: u16 = 191; - /// Middle channel value - pub const CHANNEL_VALUE_MID: u16 = 992; - /// Channel value coresponding to 2000 in betaflight - pub const CHANNEL_VALUE_2000: u16 = 1792; - /// Max channel value - pub const CHANNEL_VALUE_MAX: u16 = 1811; - - pub const CHANNEL_11_BITS: u16 = 0x7FF; - - pub(crate) const PAYLOAD_LENGTH: u8 = 22; - - pub(crate) fn parse(data: &[u8]) -> Self { - // Ensure fixed-size array to allow compiler to check - // that all literal indexes are within bounds. - let data = ref_array_start::<{ Self::PAYLOAD_LENGTH as usize }>(data).unwrap(); - - // Convert u8 to u16 to make room for bit shifting - let data: [u16; Self::PAYLOAD_LENGTH as usize] = core::array::from_fn(|i| data[i] as u16); - - // Initialize all channels to 11 high bits for masking - let mut ch = [Self::CHANNEL_11_BITS; 16]; - - ch[0] &= data[0] | data[1] << 8; - ch[1] &= data[1] >> 3 | data[2] << 5; - ch[2] &= data[2] >> 6 | data[3] << 2 | data[4] << 10; - ch[3] &= data[4] >> 1 | data[5] << 7; - ch[4] &= data[5] >> 4 | data[6] << 4; - ch[5] &= data[6] >> 7 | data[7] << 1 | data[8] << 9; - ch[6] &= data[8] >> 2 | data[9] << 6; - ch[7] &= data[9] >> 5 | data[10] << 3; - ch[8] &= data[11] | data[12] << 8; - ch[9] &= data[12] >> 3 | data[13] << 5; - ch[10] &= data[13] >> 6 | data[14] << 2 | data[15] << 10; - ch[11] &= data[15] >> 1 | data[16] << 7; - ch[12] &= data[16] >> 4 | data[17] << 4; - ch[13] &= data[17] >> 7 | data[18] << 1 | data[19] << 9; - ch[14] &= data[19] >> 2 | data[20] << 6; - ch[15] &= data[20] >> 5 | data[21] << 3; - - RcChannels(ch) - } -} - -impl Payload for RcChannels { - fn len(&self) -> u8 { - Self::PAYLOAD_LENGTH - } - - fn packet_type(&self) -> PacketType { - PacketType::RcChannelsPacked - } - - fn dump(&self, data: &mut [u8]) { - // Ensure fixed-size array to allow compiler to check - // that all literal indexes are within bounds. - let data = mut_array_start::<{ Self::PAYLOAD_LENGTH as usize }>(data).unwrap(); - - // Short-hand naming - let ch = &self.0; - - // Send correctly formatted data if all channels are within bounds - if self.0.iter().all(|ch| *ch <= Self::CHANNEL_11_BITS) { - data[0] = (ch[0]) as u8; - data[1] = (ch[0] >> 8 | ch[1] << 3) as u8; - data[2] = (ch[1] >> 5 | ch[2] << 6) as u8; - data[3] = (ch[2] >> 2) as u8; - data[4] = (ch[2] >> 10 | ch[3] << 1) as u8; - data[5] = (ch[3] >> 7 | ch[4] << 4) as u8; - data[6] = (ch[4] >> 4 | ch[5] << 7) as u8; - data[7] = (ch[5] >> 1) as u8; - data[8] = (ch[5] >> 9 | ch[6] << 2) as u8; - data[9] = (ch[6] >> 6 | ch[7] << 5) as u8; - data[10] = (ch[7] >> 3) as u8; - data[11] = (ch[8]) as u8; - data[12] = (ch[8] >> 8 | ch[9] << 3) as u8; - data[13] = (ch[9] >> 5 | ch[10] << 6) as u8; - data[14] = (ch[10] >> 2) as u8; - data[15] = (ch[10] >> 10 | ch[11] << 1) as u8; - data[16] = (ch[11] >> 7 | ch[12] << 4) as u8; - data[17] = (ch[12] >> 4 | ch[13] << 7) as u8; - data[18] = (ch[13] >> 1) as u8; - data[19] = (ch[13] >> 9 | ch[14] << 2) as u8; - data[20] = (ch[14] >> 6 | ch[15] << 5) as u8; - data[21] = (ch[15] >> 3) as u8; - - // If *any* channel is out of bounds, clamp *everything* to the maximum value - } else { - data.iter_mut().for_each(|e| *e = Self::CHANNEL_11_BITS as u8); - } - } -} - -impl core::ops::Deref for RcChannels { - type Target = [u16; 16]; - - fn deref(&self) -> &Self::Target { - &self.0 - } -} - -impl core::ops::DerefMut for RcChannels { - fn deref_mut(&mut self) -> &mut Self::Target { - &mut self.0 - } -} - -#[cfg(test)] -mod tests { - use crate::packets::Payload; - use crate::{Packet, RcChannels}; - - #[test] - fn test_rc_channels_write_and_parse() { - let mut original = RcChannels([0; 16]); - for i in 0..16 { - original[i] = i as u16 * 10; - } - - let mut data = [0u8; Packet::MAX_LENGTH]; - original.dump(&mut data); - - let parsed = RcChannels::parse(&data); - for i in 0..16 { - assert_eq!(parsed[i], i as u16 * 10); - } - } -} diff --git a/src/packets/rc_channels_packed.rs b/src/packets/rc_channels_packed.rs new file mode 100644 index 0000000..2b5b72b --- /dev/null +++ b/src/packets/rc_channels_packed.rs @@ -0,0 +1,110 @@ +//! RcChannelsPacked packet and related functions/implementations + +use super::Payload; +use crate::to_array::{mut_array_start, ref_array_start}; +use crate::{CrsfError, PacketType}; + +/// Represents a RcChannelsPacked packet +#[derive(Debug, PartialEq, Clone, Copy)] +pub struct RcChannelsPacked(pub [u16; 16]); + +const LEN: usize = RcChannelsPacked::LEN; + +/// The raw decoder (parser) for the RcChannelsPacked packet. +pub fn raw_decode(data: &[u8; LEN]) -> RcChannelsPacked { + // Convert u8 to u16 to make room for bit shifting + let data: [u16; LEN] = core::array::from_fn(|i| data[i] as u16); + + const MASK_11BIT: u16 = 0x07FF; + let mut ch = [MASK_11BIT; 16]; + + ch[0] &= data[0] | data[1] << 8; + ch[1] &= data[1] >> 3 | data[2] << 5; + ch[2] &= data[2] >> 6 | data[3] << 2 | data[4] << 10; + ch[3] &= data[4] >> 1 | data[5] << 7; + ch[4] &= data[5] >> 4 | data[6] << 4; + ch[5] &= data[6] >> 7 | data[7] << 1 | data[8] << 9; + ch[6] &= data[8] >> 2 | data[9] << 6; + ch[7] &= data[9] >> 5 | data[10] << 3; + ch[8] &= data[11] | data[12] << 8; + ch[9] &= data[12] >> 3 | data[13] << 5; + ch[10] &= data[13] >> 6 | data[14] << 2 | data[15] << 10; + ch[11] &= data[15] >> 1 | data[16] << 7; + ch[12] &= data[16] >> 4 | data[17] << 4; + ch[13] &= data[17] >> 7 | data[18] << 1 | data[19] << 9; + ch[14] &= data[19] >> 2 | data[20] << 6; + ch[15] &= data[20] >> 5 | data[21] << 3; + + RcChannelsPacked(ch) +} + +/// The raw encoder (serializer) for the RcChannelsPacked packet. +pub fn raw_encode(ch: &RcChannelsPacked, data: &mut [u8; LEN]) { + let ch = &ch.0; + + data[0] = (ch[0]) as u8; + data[1] = (ch[0] >> 8 | ch[1] << 3) as u8; + data[2] = (ch[1] >> 5 | ch[2] << 6) as u8; + data[3] = (ch[2] >> 2) as u8; + data[4] = (ch[2] >> 10 | ch[3] << 1) as u8; + data[5] = (ch[3] >> 7 | ch[4] << 4) as u8; + data[6] = (ch[4] >> 4 | ch[5] << 7) as u8; + data[7] = (ch[5] >> 1) as u8; + data[8] = (ch[5] >> 9 | ch[6] << 2) as u8; + data[9] = (ch[6] >> 6 | ch[7] << 5) as u8; + data[10] = (ch[7] >> 3) as u8; + data[11] = (ch[8]) as u8; + data[12] = (ch[8] >> 8 | ch[9] << 3) as u8; + data[13] = (ch[9] >> 5 | ch[10] << 6) as u8; + data[14] = (ch[10] >> 2) as u8; + data[15] = (ch[10] >> 10 | ch[11] << 1) as u8; + data[16] = (ch[11] >> 7 | ch[12] << 4) as u8; + data[17] = (ch[12] >> 4 | ch[13] << 7) as u8; + data[18] = (ch[13] >> 1) as u8; + data[19] = (ch[13] >> 9 | ch[14] << 2) as u8; + data[20] = (ch[14] >> 6 | ch[15] << 5) as u8; + data[21] = (ch[15] >> 3) as u8; +} + +impl Payload for RcChannelsPacked { + const LEN: usize = 22; + + fn packet_type(&self) -> PacketType { + PacketType::RcChannelsPacked + } + + fn decode(buf: &[u8]) -> Result { + let data = ref_array_start(buf).ok_or(CrsfError::BufferError)?; + + Ok(raw_decode(data)) + } + + fn encode<'a>(&self, buf: &'a mut [u8]) -> Result<&'a [u8], CrsfError> { + let data = mut_array_start(buf).ok_or(CrsfError::BufferError)?; + + raw_encode(self, data); + + Ok(data) + } +} + +#[cfg(test)] +mod tests { + use super::RcChannelsPacked; + use crate::Payload; + + #[test] + fn rc_channels_packed_encode_decode() { + let original = RcChannelsPacked([ + 983, 992, 174, 992, 191, 191, 191, 191, 997, 997, 997, 997, 0, 0, 1811, 1811, + ]); + + let raw = original.into_raw_packet().unwrap(); + + let data = raw.payload().unwrap(); + + let parsed = RcChannelsPacked::decode(&data).unwrap(); + + assert_eq!(parsed, original); + } +} diff --git a/src/raw_packet.rs b/src/raw_packet.rs new file mode 100644 index 0000000..f9cb8b9 --- /dev/null +++ b/src/raw_packet.rs @@ -0,0 +1,96 @@ +use crate::{ + address::PacketAddress, CrsfError, LinkStatistics, Packet, PacketType, Payload, + RcChannelsPacked, CRSF_MAX_LEN, +}; + +/// Represents a raw packet (not parsed) +#[derive(Clone, Copy, Debug)] +pub struct RawPacket { + pub(crate) buf: [u8; CRSF_MAX_LEN], + pub(crate) len: usize, +} + +impl RawPacket { + pub(crate) const fn empty() -> RawPacket { + RawPacket { + buf: [0u8; CRSF_MAX_LEN], + len: 0, + } + } + + /// Create a new RawPacket from the given slice. The slice must be + /// at most `CRSF_MAX_LEN`bytes long. + pub fn new(slice: &[u8]) -> Result { + let mut packet = RawPacket { + buf: [0u8; CRSF_MAX_LEN], + len: slice.len(), + }; + + packet + .buf + .get_mut(..slice.len()) + .ok_or(CrsfError::BufferError)? + .copy_from_slice(slice); + + Ok(packet) + } + + /// Get the slice of the raw packets buffer + pub fn as_slice(&self) -> &[u8] { + &self.buf[..self.len.min(CRSF_MAX_LEN)] + } + + /// Get the payload section of the raw packet + pub fn payload(&self) -> Result<&[u8], CrsfError> { + match (self.is_extended(), self.as_slice()) { + // Skip the [sync], [len], [type], [src], [dst] and [crc] bytes + (true, [_, _, _, _, _, payload @ .., _]) => Ok(payload), + // Skip the [sync], [len], [type] and [crc] bytes + (false, [_, _, _, payload @ .., _]) => Ok(payload), + _ => Err(CrsfError::BufferError), + } + } + + /// Check if the packet is an extended format packet + pub fn is_extended(&self) -> bool { + // Skip the [sync], [len], [type] and [crc] bytes + if let [_, _, ty, ..] = self.as_slice() { + ty >= &0x28 + } else { + false + } + } + + /// Get the source and destination addresses of the packet. + /// This is only valid for extended packets, and will + /// return an error otherwise + pub fn dst_src(&self) -> Result<(PacketAddress, PacketAddress), CrsfError> { + if self.is_extended() { + if let [_, _, _, dst, src, ..] = self.as_slice() { + match (PacketAddress::try_from(*dst), PacketAddress::try_from(*src)) { + (Ok(dst), Ok(src)) => Ok((dst, src)), + _ => Err(CrsfError::InvalidPayload), + } + } else { + Err(CrsfError::BufferError) + } + } else { + // NOTE Not sure what the error here should be + Err(CrsfError::UnknownType { typ: 0 }) + } + } + + /// Convert the raw packet into a parsed packet + pub fn into_packet(&self) -> Result { + let payload = self.payload()?; + match PacketType::try_from(self.buf[2]) { + Ok(PacketType::RcChannelsPacked) => { + RcChannelsPacked::decode(payload).map(Packet::RcChannelsPacked) + } + Ok(PacketType::LinkStatistics) => { + LinkStatistics::decode(payload).map(Packet::LinkStatistics) + } + _ => Err(CrsfError::UnknownType { typ: self.buf[2] }), + } + } +}