skydroid_protocol/
framing.rs

1//! The receive-side `#TP` frame parser (RX side) - allocation-free.
2//! Synchs on `#tp\0` header, parses msgid/dataLen/data/checkSum, stream-tolerant.
3
4pub const ARLINK_USR_DATA_MAX_LEN: usize = 16384;
5pub const HEADER_STREAM: [u8; 4] = [0x23, 0x74, 0x70, 0x00];
6
7#[derive(Debug, Clone, Copy, PartialEq, Eq)]
8pub struct RxFrameRef<'a> {
9    pub msgid: u8,
10    pub data_len: usize,
11    pub data: &'a [u8],
12    pub checksum: u8,
13}
14
15#[derive(Debug, Clone, Copy, PartialEq, Eq)]
16enum State { Header, MsgId, DataLen, Data, CheckSum }
17
18#[derive(Debug, Clone, Copy)]
19struct PendingFrame { data_offset: usize, msgid: u8, data_len: usize, checksum: u8 }
20
21pub struct RxDecoder<const CAP: usize> {
22    buf: [u8; CAP],
23    len: usize,
24    state: State,
25    header_idx: usize,
26    data_len: usize,
27    pending: Option<PendingFrame>,
28}
29
30impl<const CAP: usize> Default for RxDecoder<CAP> {
31    fn default() -> Self { Self::new() }
32}
33
34impl<const CAP: usize> RxDecoder<CAP> {
35    pub fn new() -> Self {
36        Self { buf: [0u8; CAP], len: 0, state: State::Header, header_idx: 0, data_len: 0, pending: None }
37    }
38
39    pub fn has_frame(&self) -> bool { self.pending.is_some() }
40
41    pub fn feed(&mut self, chunk: &[u8]) {
42        for &b in chunk { self.feed_byte(b); }
43    }
44
45    pub fn feed_byte(&mut self, byte: u8) {
46        match self.state {
47            State::Header => {
48                if self.header_idx < CAP && byte == HEADER_STREAM[self.header_idx] {
49                    self.buf[self.header_idx] = byte;
50                    self.header_idx += 1;
51                    if self.header_idx == HEADER_STREAM.len() {
52                        self.state = State::MsgId;
53                        self.len = HEADER_STREAM.len();
54                    }
55                } else if self.header_idx > 0 {
56                    self.header_idx = 0;
57                    if byte == HEADER_STREAM[0] { self.buf[0] = byte; self.header_idx = 1; }
58                } else {
59                    self.header_idx = if byte == HEADER_STREAM[0] { 1 } else { 0 };
60                }
61            }
62            State::MsgId => {
63                if self.len >= CAP { self.reset(); return; }
64                self.buf[self.len] = byte;
65                self.len += 1;
66                self.state = State::DataLen;
67            }
68            State::DataLen => {
69                let dl = byte as usize;
70                if self.len + dl + 1 > CAP {
71                    self.reset();
72                    if byte == HEADER_STREAM[0] { self.buf[0] = byte; self.header_idx = 1; }
73                    return;
74                }
75                self.buf[self.len] = byte;
76                self.len += 1;
77                self.data_len = dl;
78                self.state = if dl == 0 { State::CheckSum } else { State::Data };
79            }
80            State::Data => {
81                if self.len + 1 >= CAP { self.reset(); return; }
82                self.buf[self.len] = byte;
83                self.len += 1;
84                if self.len == HEADER_STREAM.len() + 2 + self.data_len {
85                    self.state = State::CheckSum;
86                }
87            }
88            State::CheckSum => {
89                let calc = self.buf[..self.len].iter().fold(0u8, |a, b| a.wrapping_add(*b));
90                let valid = calc == byte;
91                let pending = PendingFrame {
92                    data_offset: HEADER_STREAM.len() + 2,
93                    msgid: self.buf[HEADER_STREAM.len()],
94                    data_len: self.data_len,
95                    checksum: byte,
96                };
97                self.reset();
98                if valid { self.pending = Some(pending); }
99            }
100        }
101    }
102
103    pub fn take_ref(&mut self) -> Option<RxFrameRef<'_>> {
104        let p = self.pending.take()?;
105        Some(RxFrameRef {
106            msgid: p.msgid,
107            data_len: p.data_len,
108            data: &self.buf[p.data_offset..p.data_offset + p.data_len],
109            checksum: p.checksum,
110        })
111    }
112
113    pub fn take_packet(&mut self, out: &mut [u8]) -> Option<usize> {
114        let p = self.pending.take()?;
115        let total = HEADER_STREAM.len() + 2 + p.data_len + 1;
116        if out.len() < total { self.pending = Some(p); return None; }
117        out[..HEADER_STREAM.len()].copy_from_slice(&HEADER_STREAM);
118        out[HEADER_STREAM.len()] = p.msgid;
119        out[HEADER_STREAM.len() + 1] = p.data_len as u8;
120        out[HEADER_STREAM.len() + 2..HEADER_STREAM.len() + 2 + p.data_len]
121            .copy_from_slice(&self.buf[p.data_offset..p.data_offset + p.data_len]);
122        out[total - 1] = p.checksum;
123        Some(total)
124    }
125
126    fn reset(&mut self) {
127        self.state = State::Header;
128        self.header_idx = 0;
129        self.len = 0;
130        self.data_len = 0;
131        self.pending = None;
132    }
133}
134
135#[cfg(test)]
136mod tests {
137    use super::*;
138
139    fn make_frame(msgid: u8, payload: &[u8], out: &mut [u8]) -> usize {
140        let mut n = 0;
141        out[n..n+4].copy_from_slice(&HEADER_STREAM); n += 4;
142        out[n] = msgid; n += 1;
143        out[n] = payload.len() as u8; n += 1;
144        out[n..n+payload.len()].copy_from_slice(payload); n += payload.len();
145        let sum = out[..n].iter().fold(0u8, |a, b| a.wrapping_add(*b));
146        out[n] = sum; n + 1
147    }
148
149    #[test]
150    fn parses_single_frame() {
151        let mut f = [0u8; 64];
152        let n = make_frame(1, b"hi", &mut f);
153        let mut d = RxDecoder::<64>::new();
154        d.feed(&f[..n]);
155        assert!(d.has_frame());
156        let fr = d.take_ref().unwrap();
157        assert_eq!(fr.msgid, 1);
158        assert_eq!(fr.data, b"hi");
159    }
160
161    #[test]
162    fn tolerates_partial_chunks() {
163        let mut f = [0u8; 64];
164        let n = make_frame(7, b"payload-123456", &mut f);
165        let mut d = RxDecoder::<64>::new();
166        for &b in &f[..n] { d.feed_byte(b); }
167        let fr = d.take_ref().unwrap();
168        assert_eq!(fr.data, b"payload-123456");
169    }
170
171    #[test]
172    fn rejects_checksum_mismatch() {
173        let mut f = [0u8; 64];
174        let n = make_frame(1, b"abc", &mut f);
175        f[n-1] = f[n-1].wrapping_add(1);
176        let mut d = RxDecoder::<64>::new();
177        d.feed(&f[..n]);
178        assert!(!d.has_frame());
179        assert!(d.take_ref().is_none());
180    }
181
182    #[test]
183    fn rescans_after_garbage() {
184        let mut f = [0u8; 64];
185        let mut n = 0;
186        f[n..n+8].copy_from_slice(b"garbage!"); n += 8;
187        n += make_frame(3, b"ok", &mut f[n..]);
188        let mut d = RxDecoder::<64>::new();
189        d.feed(&f[..n]);
190        let fr = d.take_ref().unwrap();
191        assert_eq!(fr.msgid, 3);
192        assert_eq!(fr.data, b"ok");
193    }
194
195    #[test]
196    fn take_packet_writes_full_packet() {
197        let mut f = [0u8; 64];
198        let n = make_frame(9, b"xyz", &mut f);
199        let mut d = RxDecoder::<64>::new();
200        d.feed(&f[..n]);
201        let mut out = [0u8; 64];
202        let total = d.take_packet(&mut out).unwrap();
203        assert_eq!(total, 4+1+1+3+1);
204        assert_eq!(&out[..4], &HEADER_STREAM);
205        assert_eq!(out[4], 9);
206        assert_eq!(&out[6..9], b"xyz");
207    }
208
209    #[test]
210    fn rejects_frame_larger_than_cap() {
211        let mut f = [0u8; 64];
212        let mut n = 0;
213        f[n..n+4].copy_from_slice(&HEADER_STREAM); n += 4;
214        f[n] = 1; n += 1;
215        f[n] = 40; n += 1;
216        for i in 0..40u8 { f[n] = i; n += 1; }
217        let sum = f[..n].iter().fold(0u8, |a, b| a.wrapping_add(*b));
218        f[n] = sum; n += 1;
219        let mut d = RxDecoder::<16>::new();
220        d.feed(&f[..n]);
221        assert!(!d.has_frame());
222    }
223}
skydroid_protocol/framing.rs

skydroid_protocol/
framing.rs
