pub const SOH: u8 = 0x01;
pub const CR: u8 = 0x0D;
pub const MAX_LEN: usize = 94;
pub const fn tochar(x: u8) -> u8 {
x.wrapping_add(32)
}
pub const fn unchar(x: u8) -> u8 {
x.wrapping_sub(32)
}
pub const fn ctl(x: u8) -> u8 {
x ^ 64
}
#[derive(Clone, Copy, Debug, PartialEq, Eq, Default)]
#[non_exhaustive]
pub enum BlockCheck {
Type1,
Type2,
#[default]
Type3,
}
impl BlockCheck {
#[allow(clippy::len_without_is_empty)]
pub fn len(self) -> usize {
match self {
BlockCheck::Type1 => 1,
BlockCheck::Type2 => 2,
BlockCheck::Type3 => 3,
}
}
pub fn from_char(c: u8) -> Option<Self> {
match c {
b'1' => Some(BlockCheck::Type1),
b'2' => Some(BlockCheck::Type2),
b'3' => Some(BlockCheck::Type3),
_ => None,
}
}
pub fn to_char(self) -> u8 {
match self {
BlockCheck::Type1 => b'1',
BlockCheck::Type2 => b'2',
BlockCheck::Type3 => b'3',
}
}
pub fn compute(self, body: &[u8]) -> Vec<u8> {
match self {
BlockCheck::Type1 => {
let s: u32 = body.iter().map(|&b| u32::from(b)).sum();
vec![tochar(((s + ((s & 192) >> 6)) & 63) as u8)]
}
BlockCheck::Type2 => {
let s: u32 = body.iter().map(|&b| u32::from(b)).sum::<u32>() & 0xFFF;
vec![tochar(((s >> 6) & 63) as u8), tochar((s & 63) as u8)]
}
BlockCheck::Type3 => {
let crc = crc16(body);
vec![
tochar(((crc >> 12) & 15) as u8),
tochar(((crc >> 6) & 63) as u8),
tochar((crc & 63) as u8),
]
}
}
}
}
pub fn crc16(body: &[u8]) -> u16 {
let mut crc: u16 = 0;
for &b in body {
let b = u16::from(b);
crc = (crc >> 4) ^ (((crc ^ b) & 15) * 0x1081);
crc = (crc >> 4) ^ (((crc ^ (b >> 4)) & 15) * 0x1081);
}
crc
}
#[derive(Clone, Debug, PartialEq, Eq)]
pub struct Packet {
pub seq: u8,
pub kind: u8,
pub data: Vec<u8>,
}
impl Packet {
pub fn new(seq: u8, kind: u8, data: Vec<u8>) -> Self {
Packet {
seq: seq % 64,
kind,
data,
}
}
pub fn max_data(maxl: usize, check: BlockCheck) -> usize {
maxl.min(MAX_LEN).saturating_sub(2 + check.len())
}
pub fn encode(&self, check: BlockCheck, framing: &Framing) -> Result<Vec<u8>, EncodeError> {
let max = Packet::max_data(MAX_LEN, check);
if self.data.len() > max {
return Err(EncodeError {
len: self.data.len(),
max,
});
}
Ok(self.wire(check, framing))
}
pub(crate) fn wire(&self, check: BlockCheck, framing: &Framing) -> Vec<u8> {
let len = 2 + self.data.len() + check.len();
debug_assert!(
len <= MAX_LEN,
"packet data too long: LEN {len} exceeds {MAX_LEN}"
);
let len_char = u8::try_from(len.min(MAX_LEN)).map_or(tochar(94), tochar);
let mut out = Vec::with_capacity(usize::from(framing.npad) + len + 3);
out.extend(std::iter::repeat_n(framing.padc, usize::from(framing.npad)));
out.push(SOH);
let body_start = out.len();
out.push(len_char);
out.push(tochar(self.seq % 64));
out.push(self.kind);
out.extend_from_slice(&self.data);
let chk = check.compute(&out[body_start..]);
out.extend_from_slice(&chk);
out.push(framing.eol);
out
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct EncodeError {
pub len: usize,
pub max: usize,
}
impl std::fmt::Display for EncodeError {
fn fmt(&self, f: &mut std::fmt::Formatter<'_>) -> std::fmt::Result {
write!(
f,
"packet data too long: {} bytes, at most {} fit",
self.len, self.max
)
}
}
impl std::error::Error for EncodeError {}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
pub struct Framing {
pub npad: u8,
pub padc: u8,
pub eol: u8,
}
impl Default for Framing {
fn default() -> Self {
Framing {
npad: 0,
padc: 0,
eol: CR,
}
}
}
#[derive(Clone, Copy, Debug, PartialEq, Eq)]
#[non_exhaustive]
pub enum FrameError {
BadLength,
BadCheck,
}
#[derive(Debug, Default)]
pub struct Deframer {
buf: Vec<u8>,
}
impl Deframer {
pub fn new() -> Self {
Self::default()
}
pub fn push(&mut self, bytes: &[u8]) {
self.buf.extend_from_slice(bytes);
}
pub fn next_frame(&mut self) -> Option<Vec<u8>> {
let mut from = 0;
loop {
let Some(s) = self
.buf
.get(from..)
.and_then(|rest| rest.iter().position(|&b| b == SOH))
.map(|i| from + i)
else {
self.buf.clear();
return None;
};
let Some(&len_byte) = self.buf.get(s + 1) else {
self.buf.drain(..s);
return None;
};
let len = usize::from(unchar(len_byte));
if !(3..=94).contains(&len) {
from = s + 1;
continue;
}
let total = 2 + len;
let end = (s + total).min(self.buf.len());
if let Some(j) = self.buf[s + 1..end].iter().position(|&b| b == SOH) {
from = s + 1 + j;
continue;
}
if self.buf.len() < s + total {
self.buf.drain(..s);
return None;
}
let frame = self.buf[s..s + total].to_vec();
self.buf.drain(..s + total);
return Some(frame);
}
}
pub fn clear(&mut self) {
self.buf.clear();
}
}
pub fn parse_frame(frame: &[u8], check: BlockCheck) -> Result<Packet, FrameError> {
if frame.len() < 5 || frame[0] != SOH {
return Err(FrameError::BadLength);
}
let len = usize::from(unchar(frame[1]));
if !(3..=94).contains(&len) || frame.len() != 2 + len {
return Err(FrameError::BadLength);
}
let kind = frame[3];
let check = if kind == b'N' {
match len - 2 {
1 => BlockCheck::Type1,
2 => BlockCheck::Type2,
3 => BlockCheck::Type3,
_ => check,
}
} else {
check
};
let cl = check.len();
if len < 2 + cl {
return Err(FrameError::BadLength);
}
let split = frame.len() - cl;
if check.compute(&frame[1..split]) != frame[split..] {
return Err(FrameError::BadCheck);
}
Ok(Packet::new(
unchar(frame[2]),
kind,
frame[4..split].to_vec(),
))
}
#[cfg(test)]
#[allow(clippy::unwrap_used, clippy::expect_used)]
mod tests {
use super::*;
const VECTORS: &[(&[u8], u8, u8, &[u8])] = &[
(b"\x01, I~* @-#Y3~Z\r", 0, b'I', b"~* @-#Y3~"),
(b"\x01+ Y~& @-# 3,\r", 0, b'Y', b"~& @-# 3"),
(b"\x01( C6 7 *C\r", 0, b'C', b"6 7 *"),
(b"\x01+ S~* @-#Y3$\r", 0, b'S', b"~* @-#Y3"),
(b"\x01# N3\r", 0, b'N', b""),
(b"\x01#!X>\r", 1, b'X', b""),
(b"\x01#!Y?\r", 1, b'Y', b""),
(
b"\x01?\"D1: 42#M#J6\r",
2,
b'D',
b"1: 42#M#J",
),
];
#[test]
fn char_transforms() {
assert_eq!(tochar(0), b' ');
assert_eq!(tochar(94), b'~');
assert_eq!(unchar(b'~'), 94);
assert_eq!(unchar(tochar(17)), 17);
assert_eq!(ctl(0), b'@');
assert_eq!(ctl(b'M'), CR);
assert_eq!(ctl(0x7F), b'?');
}
#[test]
fn crc_vector() {
assert_eq!(crc16(b"123456789"), 0x2189);
assert_eq!(crc16(b""), 0);
}
#[test]
fn check_types() {
assert_eq!(BlockCheck::Type1.compute(b"# N"), b"3");
let s: u32 = b"abc".iter().map(|&b| u32::from(b)).sum();
assert_eq!(
BlockCheck::Type2.compute(b"abc"),
vec![tochar(((s >> 6) & 63) as u8), tochar((s & 63) as u8)]
);
let c = crc16(b"123456789");
assert_eq!(
BlockCheck::Type3.compute(b"123456789"),
vec![
tochar((c >> 12) as u8 & 15),
tochar((c >> 6) as u8 & 63),
tochar(c as u8 & 63)
]
);
for t in [BlockCheck::Type1, BlockCheck::Type2, BlockCheck::Type3] {
assert_eq!(BlockCheck::from_char(t.to_char()), Some(t));
assert_eq!(t.compute(b"xyz").len(), t.len());
}
assert_eq!(BlockCheck::from_char(b'4'), None);
assert_eq!(BlockCheck::default(), BlockCheck::Type3);
}
#[test]
fn wire_vectors_round_trip() {
for &(wire, seq, kind, data) in VECTORS {
let frame = &wire[..wire.len() - 1];
let p = parse_frame(frame, BlockCheck::Type1).unwrap();
assert_eq!(p, Packet::new(seq, kind, data.to_vec()), "{wire:?}");
assert_eq!(
p.encode(BlockCheck::Type1, &Framing::default()),
Ok(wire.to_vec())
);
}
}
#[test]
fn type2_type3_round_trip() {
let p = Packet::new(70, b'D', b"hello #M#J world".to_vec());
assert_eq!(p.seq, 6);
for t in [BlockCheck::Type1, BlockCheck::Type2, BlockCheck::Type3] {
let wire = p.encode(t, &Framing::default()).unwrap();
let frame = &wire[..wire.len() - 1];
assert_eq!(parse_frame(frame, t).unwrap(), p);
}
}
#[test]
fn max_data_and_longest_packet() {
assert_eq!(Packet::max_data(80, BlockCheck::Type1), 77);
assert_eq!(Packet::max_data(94, BlockCheck::Type3), 89);
assert_eq!(Packet::max_data(200, BlockCheck::Type3), 89);
assert_eq!(Packet::max_data(3, BlockCheck::Type3), 0);
let p = Packet::new(0, b'D', vec![b'x'; 89]);
let wire = p.encode(BlockCheck::Type3, &Framing::default()).unwrap();
assert_eq!(wire[1], b'~');
let frame = &wire[..wire.len() - 1];
assert_eq!(parse_frame(frame, BlockCheck::Type3).unwrap(), p);
}
#[test]
fn oversized_packet_is_an_error() {
let p = Packet::new(0, b'D', vec![b'x'; 90]);
let err = p.encode(BlockCheck::Type3, &Framing::default());
assert_eq!(err, Err(EncodeError { len: 90, max: 89 }));
let p = Packet::new(0, b'D', vec![b'x'; 91]);
assert!(p.encode(BlockCheck::Type1, &Framing::default()).is_ok());
let p = Packet::new(0, b'D', vec![b'x'; 92]);
assert!(p.encode(BlockCheck::Type1, &Framing::default()).is_err());
}
#[test]
fn padding_and_eol() {
let f = Framing {
npad: 2,
padc: 0,
eol: b'\n',
};
let wire = Packet::new(1, b'Y', vec![])
.encode(BlockCheck::Type1, &f)
.unwrap();
assert_eq!(wire, b"\0\0\x01#!Y?\n");
}
#[test]
fn bad_check_detected() {
let wire = Packet::new(3, b'D', b"abcdef".to_vec())
.encode(BlockCheck::Type3, &Framing::default())
.unwrap();
let mut frame = wire[..wire.len() - 1].to_vec();
frame[6] ^= 1;
assert_eq!(
parse_frame(&frame, BlockCheck::Type3),
Err(FrameError::BadCheck)
);
}
#[test]
fn bad_length() {
assert_eq!(
parse_frame(b"\x01$ Y?", BlockCheck::Type1),
Err(FrameError::BadLength)
);
assert_eq!(
parse_frame(b"\x01\" Y?", BlockCheck::Type1),
Err(FrameError::BadLength)
);
assert_eq!(
parse_frame(b"\x01# Y?", BlockCheck::Type3),
Err(FrameError::BadLength)
);
}
#[test]
fn nak_check_length_heuristic() {
let nak = Packet::new(5, b'N', vec![]);
for t in [BlockCheck::Type1, BlockCheck::Type2, BlockCheck::Type3] {
let wire = nak.encode(t, &Framing::default()).unwrap();
let frame = &wire[..wire.len() - 1];
assert_eq!(parse_frame(frame, BlockCheck::Type1).unwrap(), nak);
assert_eq!(parse_frame(frame, BlockCheck::Type3).unwrap(), nak);
}
}
fn drain(d: &mut Deframer) -> Vec<Vec<u8>> {
let mut v = Vec::new();
while let Some(f) = d.next_frame() {
v.push(f);
}
v
}
#[test]
fn deframer_byte_by_byte() {
let mut d = Deframer::new();
let mut frames = Vec::new();
let input: Vec<u8> = VECTORS.iter().flat_map(|v| v.0.iter().copied()).collect();
for b in input {
d.push(&[b]);
frames.extend(drain(&mut d));
}
assert_eq!(frames.len(), VECTORS.len());
for (f, v) in frames.iter().zip(VECTORS) {
assert_eq!(f.as_slice(), &v.0[..v.0.len() - 1]);
}
}
#[test]
fn deframer_garbage_and_two_packets() {
let mut d = Deframer::new();
d.push(b"noise\r\n\x01# N3\r\x01#!Y?\r");
assert_eq!(
drain(&mut d),
vec![b"\x01# N3".to_vec(), b"\x01#!Y?".to_vec()]
);
d.push(b"only garbage");
assert_eq!(d.next_frame(), None);
}
#[test]
fn deframer_invalid_len_skipped() {
let mut d = Deframer::new();
d.push(b"\x01\x7f\x01\"x\x01#!Y?\r");
assert_eq!(drain(&mut d), vec![b"\x01#!Y?".to_vec()]);
}
#[test]
fn deframer_resync_on_truncated() {
let mut d = Deframer::new();
d.push(b"\x01?\"D1: ");
assert_eq!(d.next_frame(), None);
d.push(b"\x01#!Y?\r");
assert_eq!(drain(&mut d), vec![b"\x01#!Y?".to_vec()]);
}
#[test]
fn deframer_soh_run_is_linear() {
let mut d = Deframer::new();
d.push(&vec![SOH; 1 << 20]);
let t = std::time::Instant::now();
assert_eq!(d.next_frame(), None);
assert!(t.elapsed() < std::time::Duration::from_secs(2));
d.push(b"#!Y?\r");
assert_eq!(drain(&mut d), vec![b"\x01#!Y?".to_vec()]);
let mut d = Deframer::new();
d.push(&b"\x01~".repeat(1 << 19));
let t = std::time::Instant::now();
assert_eq!(d.next_frame(), None);
assert!(t.elapsed() < std::time::Duration::from_secs(2));
}
#[test]
fn deframer_clear() {
let mut d = Deframer::new();
d.push(b"\x01#!Y");
d.clear();
d.push(b"?\r");
assert_eq!(d.next_frame(), None);
}
}