1use thiserror::Error;
8
9pub const SOF: u8 = 0xFE;
11pub const MAX_DATA: usize = 250;
13
14#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
16pub enum Kind {
17 Poll,
19 Sreq,
21 Areq,
23 Srsp,
25}
26
27impl Kind {
28 fn bits(self) -> u8 {
29 match self {
30 Kind::Poll => 0x00,
31 Kind::Sreq => 0x20,
32 Kind::Areq => 0x40,
33 Kind::Srsp => 0x60,
34 }
35 }
36
37 fn from_bits(b: u8) -> Option<Kind> {
38 match b & 0xE0 {
39 0x00 => Some(Kind::Poll),
40 0x20 => Some(Kind::Sreq),
41 0x40 => Some(Kind::Areq),
42 0x60 => Some(Kind::Srsp),
43 _ => None,
44 }
45 }
46}
47
48#[derive(Debug, Clone, PartialEq, Eq)]
50pub struct Frame {
51 pub kind: Kind,
53 pub subsystem: u8,
55 pub id: u8,
57 pub data: Vec<u8>,
59}
60
61#[derive(Debug, Clone, Copy, PartialEq, Eq, Error)]
63pub enum FrameError {
64 #[error("MT data of {0} bytes is over the 250 byte limit")]
66 TooLong(usize),
67 #[error("subsystem {0:#04x} does not fit five bits")]
69 Subsystem(u8),
70}
71
72impl Frame {
73 pub fn new(kind: Kind, subsystem: u8, id: u8, data: Vec<u8>) -> Frame {
75 Frame {
76 kind,
77 subsystem,
78 id,
79 data,
80 }
81 }
82
83 pub fn command(&self) -> (u8, u8) {
85 (self.kind.bits() | (self.subsystem & 0x1F), self.id)
86 }
87
88 pub fn encode(&self) -> Result<Vec<u8>, FrameError> {
90 if self.data.len() > MAX_DATA {
91 return Err(FrameError::TooLong(self.data.len()));
92 }
93 if self.subsystem > 0x1F {
94 return Err(FrameError::Subsystem(self.subsystem));
95 }
96 let (cmd0, cmd1) = self.command();
97 let mut out = Vec::with_capacity(self.data.len() + 5);
98 out.push(SOF);
99 out.push(self.data.len() as u8);
100 out.push(cmd0);
101 out.push(cmd1);
102 out.extend_from_slice(&self.data);
103 let fcs = out.iter().skip(1).fold(0u8, |a, b| a ^ b);
104 out.push(fcs);
105 Ok(out)
106 }
107}
108
109#[derive(Debug, Clone, PartialEq, Eq)]
111pub enum Decoded {
112 Frame(Frame),
114 BadFcs,
116}
117
118#[derive(Debug, Default)]
121pub struct Decoder {
122 buf: Vec<u8>,
123}
124
125impl Decoder {
126 pub fn new() -> Decoder {
128 Decoder::default()
129 }
130
131 pub fn push(&mut self, bytes: &[u8]) -> Vec<Decoded> {
133 let mut out = Vec::new();
134 for &b in bytes {
135 if self.buf.is_empty() {
136 if b == SOF {
137 self.buf.push(b);
138 }
139 continue;
140 }
141 if self.buf.len() == 1 && usize::from(b) > MAX_DATA {
142 self.buf.clear();
144 if b == SOF {
145 self.buf.push(b);
146 }
147 continue;
148 }
149 self.buf.push(b);
150 let Some(&len) = self.buf.get(1) else {
151 continue;
152 };
153 let total = usize::from(len) + 5;
154 if self.buf.len() < total {
155 continue;
156 }
157 let frame = std::mem::take(&mut self.buf);
158 out.push(parse_whole(&frame));
159 }
160 out
161 }
162}
163
164fn parse_whole(frame: &[u8]) -> Decoded {
165 let body = frame.get(1..frame.len().saturating_sub(1)).unwrap_or(&[]);
166 let fcs = frame.last().copied().unwrap_or(0);
167 if body.iter().fold(0u8, |a, b| a ^ b) != fcs {
168 return Decoded::BadFcs;
169 }
170 match (body.get(1), body.get(2), body.get(3..)) {
171 (Some(&cmd0), Some(&cmd1), Some(data)) => match Kind::from_bits(cmd0) {
172 Some(kind) => Decoded::Frame(Frame {
173 kind,
174 subsystem: cmd0 & 0x1F,
175 id: cmd1,
176 data: data.to_vec(),
177 }),
178 None => Decoded::BadFcs,
179 },
180 _ => Decoded::BadFcs,
181 }
182}
183
184#[cfg(test)]
185mod tests {
186 use super::*;
187
188 #[test]
189 fn encodes_sys_ping() {
190 let f = Frame::new(Kind::Sreq, 0x01, 0x01, vec![]);
192 assert_eq!(f.encode().unwrap(), [0xFE, 0x00, 0x21, 0x01, 0x20]);
193 }
194
195 #[test]
196 fn decodes_across_chunks_and_noise() {
197 let f = Frame::new(Kind::Srsp, 0x01, 0x01, vec![0x79, 0x07]);
198 let bytes = f.encode().unwrap();
199 let mut d = Decoder::new();
200 assert!(d.push(&[0x00, 0x13]).is_empty());
201 assert!(d.push(&bytes[..3]).is_empty());
202 assert_eq!(d.push(&bytes[3..]), vec![Decoded::Frame(f.clone())]);
203 let mut bad = bytes.clone();
204 if let Some(last) = bad.last_mut() {
205 *last ^= 1;
206 }
207 bad.extend_from_slice(&bytes);
208 assert_eq!(d.push(&bad), vec![Decoded::BadFcs, Decoded::Frame(f)]);
209 }
210
211 #[test]
212 fn refuses_long_frames() {
213 let f = Frame::new(Kind::Sreq, 1, 1, vec![0; 251]);
214 assert_eq!(f.encode(), Err(FrameError::TooLong(251)));
215 let mut d = Decoder::new();
216 assert!(d.push(&[0xFE, 0xFF, 0xFE, 0x00, 0x21, 0x01, 0x20]).len() == 1);
217 }
218}