use thiserror::Error;
pub const SOF: u8 = 0xFE;
pub const MAX_DATA: usize = 250;
#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
pub enum Kind {
Poll,
Sreq,
Areq,
Srsp,
}
impl Kind {
fn bits(self) -> u8 {
match self {
Kind::Poll => 0x00,
Kind::Sreq => 0x20,
Kind::Areq => 0x40,
Kind::Srsp => 0x60,
}
}
fn from_bits(b: u8) -> Option<Kind> {
match b & 0xE0 {
0x00 => Some(Kind::Poll),
0x20 => Some(Kind::Sreq),
0x40 => Some(Kind::Areq),
0x60 => Some(Kind::Srsp),
_ => None,
}
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub struct Frame {
pub kind: Kind,
pub subsystem: u8,
pub id: u8,
pub data: Vec<u8>,
}
#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
pub enum FrameError {
#[error("MT data of {0} bytes is over the 250 byte limit")]
TooLong(usize),
#[error("subsystem {0:#04x} does not fit five bits")]
Subsystem(u8),
}
impl Frame {
pub fn new(kind: Kind, subsystem: u8, id: u8, data: Vec<u8>) -> Frame {
Frame {
kind,
subsystem,
id,
data,
}
}
pub fn command(&self) -> (u8, u8) {
(self.kind.bits() | (self.subsystem & 0x1F), self.id)
}
pub fn encode(&self) -> Result<Vec<u8>, FrameError> {
if self.data.len() > MAX_DATA {
return Err(FrameError::TooLong(self.data.len()));
}
if self.subsystem > 0x1F {
return Err(FrameError::Subsystem(self.subsystem));
}
let (cmd0, cmd1) = self.command();
let mut out = Vec::with_capacity(self.data.len() + 5);
out.push(SOF);
out.push(self.data.len() as u8);
out.push(cmd0);
out.push(cmd1);
out.extend_from_slice(&self.data);
let fcs = out.iter().skip(1).fold(0u8, |a, b| a ^ b);
out.push(fcs);
Ok(out)
}
}
#[derive(Debug, Clone, PartialEq, Eq)]
pub enum Decoded {
Frame(Frame),
BadFcs,
}
#[derive(Debug, Default)]
pub struct Decoder {
buf: Vec<u8>,
}
impl Decoder {
pub fn new() -> Decoder {
Decoder::default()
}
pub fn push(&mut self, bytes: &[u8]) -> Vec<Decoded> {
let mut out = Vec::new();
for &b in bytes {
if self.buf.is_empty() {
if b == SOF {
self.buf.push(b);
}
continue;
}
if self.buf.len() == 1 && usize::from(b) > MAX_DATA {
self.buf.clear();
if b == SOF {
self.buf.push(b);
}
continue;
}
self.buf.push(b);
let Some(&len) = self.buf.get(1) else {
continue;
};
let total = usize::from(len) + 5;
if self.buf.len() < total {
continue;
}
let frame = std::mem::take(&mut self.buf);
out.push(parse_whole(&frame));
}
out
}
}
fn parse_whole(frame: &[u8]) -> Decoded {
let body = frame.get(1..frame.len().saturating_sub(1)).unwrap_or(&[]);
let fcs = frame.last().copied().unwrap_or(0);
if body.iter().fold(0u8, |a, b| a ^ b) != fcs {
return Decoded::BadFcs;
}
match (body.get(1), body.get(2), body.get(3..)) {
(Some(&cmd0), Some(&cmd1), Some(data)) => match Kind::from_bits(cmd0) {
Some(kind) => Decoded::Frame(Frame {
kind,
subsystem: cmd0 & 0x1F,
id: cmd1,
data: data.to_vec(),
}),
None => Decoded::BadFcs,
},
_ => Decoded::BadFcs,
}
}
#[cfg(test)]
mod tests {
use super::*;
#[test]
fn encodes_sys_ping() {
let f = Frame::new(Kind::Sreq, 0x01, 0x01, vec![]);
assert_eq!(f.encode().unwrap(), [0xFE, 0x00, 0x21, 0x01, 0x20]);
}
#[test]
fn decodes_across_chunks_and_noise() {
let f = Frame::new(Kind::Srsp, 0x01, 0x01, vec![0x79, 0x07]);
let bytes = f.encode().unwrap();
let mut d = Decoder::new();
assert!(d.push(&[0x00, 0x13]).is_empty());
assert!(d.push(&bytes[..3]).is_empty());
assert_eq!(d.push(&bytes[3..]), vec![Decoded::Frame(f.clone())]);
let mut bad = bytes.clone();
if let Some(last) = bad.last_mut() {
*last ^= 1;
}
bad.extend_from_slice(&bytes);
assert_eq!(d.push(&bad), vec![Decoded::BadFcs, Decoded::Frame(f)]);
}
#[test]
fn refuses_long_frames() {
let f = Frame::new(Kind::Sreq, 1, 1, vec![0; 251]);
assert_eq!(f.encode(), Err(FrameError::TooLong(251)));
let mut d = Decoder::new();
assert!(d.push(&[0xFE, 0xFF, 0xFE, 0x00, 0x21, 0x01, 0x20]).len() == 1);
}
}