ados_sensorhub/
lib.rs

1#![doc = include_str!("../README.md")]
2#![no_std]
3#![warn(missing_docs)]
4#![forbid(unsafe_code)]
5
6pub mod cobs;
7mod codec;
8pub mod crc;
9pub mod packet;
10mod wire;
11
12pub use packet::*;
13
14/// Largest payload (`len`) the protocol allows.
15pub const MAX_PAYLOAD: usize = 1024;
16/// Raw frame overhead: type (1) + len (2) + crc (2).
17pub const RAW_OVERHEAD: usize = 5;
18/// Largest raw (pre-COBS) frame.
19pub const MAX_RAW: usize = MAX_PAYLOAD + RAW_OVERHEAD;
20/// Largest wire frame including the `0x00` delimiter.
21pub const MAX_FRAME: usize = cobs::max_encoded_len(MAX_RAW) + 1;
22
23/// Framing and parsing errors.
24#[derive(Debug, Clone, Copy, PartialEq, Eq)]
25pub enum Error {
26    /// The output buffer cannot hold the result.
27    BufferTooSmall,
28    /// Malformed COBS block.
29    Cobs,
30    /// `len` above [`MAX_PAYLOAD`], or decoded length ≠ `len + 5`.
31    Length,
32    /// CRC mismatch.
33    Crc,
34    /// The payload size does not fit the packet type.
35    PayloadSize {
36        /// The packet type byte.
37        ty: u8,
38    },
39    /// `ImuBatch.n` outside 1..=64.
40    ImuCount,
41}
42
43impl core::fmt::Display for Error {
44    fn fmt(&self, f: &mut core::fmt::Formatter<'_>) -> core::fmt::Result {
45        match self {
46            Self::BufferTooSmall => f.write_str("buffer too small"),
47            Self::Cobs => f.write_str("malformed COBS block"),
48            Self::Length => f.write_str("frame length mismatch"),
49            Self::Crc => f.write_str("CRC mismatch"),
50            Self::PayloadSize { ty } => write!(f, "bad payload size for type 0x{ty:02x}"),
51            Self::ImuCount => f.write_str("IMU batch sample count outside 1..=64"),
52        }
53    }
54}
55
56impl core::error::Error for Error {}
57
58/// Write the raw frame (type, len, payload, crc) of `pkt` into `out`;
59/// returns its length.
60pub fn encode_raw(pkt: &Packet<'_>, out: &mut [u8]) -> Result<usize, Error> {
61    let len = pkt.payload_len()?;
62    let total = len + RAW_OVERHEAD;
63    if out.len() < total {
64        return Err(Error::BufferTooSmall);
65    }
66    out[0] = pkt.ty();
67    // payload_len() bounds len to MAX_PAYLOAD.
68    out[1..3].copy_from_slice(&(len as u16).to_le_bytes());
69    pkt.write_payload(&mut out[3..3 + len]);
70    let crc = crc::crc16(&out[..3 + len]);
71    out[3 + len..total].copy_from_slice(&crc.to_le_bytes());
72    Ok(total)
73}
74
75/// Encode `pkt` as one wire frame (COBS + `0x00`) into `out`, using
76/// `scratch` (≥ the raw length, [`MAX_RAW`] always suffices) for the raw
77/// frame. Returns the wire length including the delimiter.
78pub fn encode_frame(pkt: &Packet<'_>, scratch: &mut [u8], out: &mut [u8]) -> Result<usize, Error> {
79    let raw = encode_raw(pkt, scratch)?;
80    let n = cobs::encode(&scratch[..raw], out)?;
81    let slot = out.get_mut(n).ok_or(Error::BufferTooSmall)?;
82    *slot = 0;
83    Ok(n + 1)
84}
85
86/// Parse a raw frame (type, len, payload, crc).
87pub fn decode_raw(raw: &[u8]) -> Result<Packet<'_>, Error> {
88    if raw.len() < RAW_OVERHEAD {
89        return Err(Error::Length);
90    }
91    let len = usize::from(u16::from_le_bytes([raw[1], raw[2]]));
92    if len > MAX_PAYLOAD || raw.len() != len + RAW_OVERHEAD {
93        return Err(Error::Length);
94    }
95    let crc = u16::from_le_bytes([raw[3 + len], raw[4 + len]]);
96    if crc::crc16(&raw[..3 + len]) != crc {
97        return Err(Error::Crc);
98    }
99    Packet::parse(raw[0], &raw[3..3 + len])
100}
101
102/// Decode one COBS block (delimiter stripped) into `buf` and parse it.
103pub fn decode_frame<'b>(block: &[u8], buf: &'b mut [u8]) -> Result<Packet<'b>, Error> {
104    let n = cobs::decode(block, buf)?;
105    decode_raw(&buf[..n])
106}
107
108/// Splits a byte stream into COBS blocks on `0x00`. A block longer than `N`
109/// is discarded up to the next delimiter (resync).
110pub struct Deframer<const N: usize> {
111    buf: [u8; N],
112    len: usize,
113    overflow: bool,
114}
115
116impl<const N: usize> Default for Deframer<N> {
117    fn default() -> Self {
118        Self::new()
119    }
120}
121
122/// Outcome of pushing one byte into a [`Deframer`].
123#[derive(Debug, PartialEq, Eq)]
124pub enum Push<'a> {
125    /// Mid-frame.
126    Pending,
127    /// A complete COBS block (delimiter stripped), ready for
128    /// [`decode_frame`].
129    Block(&'a [u8]),
130    /// A block exceeded the buffer and was dropped.
131    Overflow,
132}
133
134impl<const N: usize> Deframer<N> {
135    /// An empty deframer with an `N`-byte block buffer. Size `N` to the
136    /// largest COBS block you expect: [`MAX_FRAME`] holds any valid frame.
137    pub const fn new() -> Self {
138        Self {
139            buf: [0; N],
140            len: 0,
141            overflow: false,
142        }
143    }
144
145    /// Feed one byte from the stream. On a `0x00` delimiter this returns the
146    /// buffered block as [`Push::Block`], or [`Push::Overflow`] if the block
147    /// outgrew the buffer; an empty block (two delimiters in a row) returns
148    /// [`Push::Pending`]. Every other byte is buffered and returns
149    /// [`Push::Pending`]. The returned block borrows the deframer until the
150    /// next push.
151    pub fn push(&mut self, b: u8) -> Push<'_> {
152        if b == 0 {
153            let (len, overflow) = (self.len, self.overflow);
154            self.len = 0;
155            self.overflow = false;
156            return match (overflow, len) {
157                (true, _) => Push::Overflow,
158                (false, 0) => Push::Pending,
159                (false, n) => Push::Block(&self.buf[..n]),
160            };
161        }
162        if self.len == N {
163            self.overflow = true;
164        } else if !self.overflow {
165            self.buf[self.len] = b;
166            self.len += 1;
167        }
168        Push::Pending
169    }
170}
ados_sensorhub/lib.rs

ados_sensorhub/
lib.rs
