defmt/encoding/
rzcobs.rs

1// Standard COBS/rCOBS:
2//   00000000 => end of frame
3//   nnnnnnnn => output n-1 bytes from stream, output 0x00
4//   11111111 => output 254 bytes from stream
5//
6// zCOBS/rzCOBS:
7//   00000000 => end of frame
8//   0xxxxxxx => foreach x from LSB to MSB: if x=0 output 1 byte from stream, if x=1 output 0x00
9//   1nnnnnnn => output n+7 bytes from stream, output 0x00
10//   11111111 => output 134 bytes from stream
11
12pub(crate) struct Encoder {
13    run: u8,
14    zeros: u8,
15    started: bool,
16}
17
18impl Encoder {
19    pub const fn new() -> Self {
20        Self {
21            run: 0,
22            zeros: 0,
23            started: false,
24        }
25    }
26
27    pub fn start_frame(&mut self, mut write: impl FnMut(&[u8])) {
28        let mut write_byte = move |b: u8| write(&[b]);
29        if !self.started {
30            self.started = true;
31
32            // Write a frame-separator at the very beginning. This allows the
33            // decoder to correctly decode the first frame if a previous boot had left a
34            // partly-written frame.
35            write_byte(0x00);
36        }
37    }
38
39    pub fn end_frame(&mut self, mut write: impl FnMut(&[u8])) {
40        let mut write_byte = move |b: u8| write(&[b]);
41
42        // Finish writing the previous symbol if needed.
43        match self.run {
44            0 => {}
45            1..=6 => write_byte((self.zeros | (0xFF << self.run)) & 0x7F),
46            _ => write_byte((self.run - 7) | 0x80),
47        }
48
49        // Write frame-separator.
50        write_byte(0x00);
51
52        self.run = 0;
53        self.zeros = 0;
54    }
55
56    pub fn write(&mut self, data: &[u8], mut write: impl FnMut(&[u8])) {
57        let mut write_byte = move |b: u8| write(&[b]);
58
59        for &byte in data {
60            if self.run < 7 {
61                if byte == 0 {
62                    self.zeros |= 1 << self.run;
63                } else {
64                    write_byte(byte);
65                }
66
67                self.run += 1;
68                if self.run == 7 && self.zeros != 0x00 {
69                    write_byte(self.zeros);
70                    self.run = 0;
71                    self.zeros = 0;
72                }
73            } else if byte == 0 {
74                write_byte((self.run - 7) | 0x80);
75                self.run = 0;
76                self.zeros = 0;
77            } else {
78                write_byte(byte);
79                self.run += 1;
80                if self.run == 134 {
81                    write_byte(0xFF);
82                    self.run = 0;
83                    self.zeros = 0;
84                }
85            }
86        }
87    }
88}
89
90#[cfg(feature = "unstable-test")]
91#[cfg(test)]
92mod tests {
93    use super::*;
94
95    #[test]
96    fn it_works() {
97        let tests: &[(&[u8], &[u8])] = &[
98            (&[], &[0x00]),
99            (&[0x00], &[0x7f, 0x00]),
100            (&[0x00, 0x00], &[0x7f, 0x00]),
101            (&[0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00], &[0x7f, 0x00]),
102            (
103                &[0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00],
104                &[0x7f, 0x7f, 0x00],
105            ),
106            (&[0x01], &[0x01, 0x7e, 0x00]),
107            (&[0x01, 0x00], &[0x01, 0x7e, 0x00]),
108            (&[0x00, 0x01], &[0x01, 0x7d, 0x00]),
109            (&[0x01, 0x02], &[0x01, 0x02, 0x7c, 0x00]),
110            (
111                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x00],
112                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x40, 0x00],
113            ),
114            (
115                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77],
116                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x80, 0x00],
117            ),
118            (
119                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x00],
120                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x80, 0x00],
121            ),
122            (
123                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88],
124                &[0x11, 0x22, 0x33, 0x44, 0x55, 0x66, 0x77, 0x88, 0x81, 0x00],
125            ),
126            (
127                &[
128                    0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
129                    0xff,
130                ],
131                &[0x7f, 0xff, 0x3f, 0x00],
132            ),
133            (
134                &[
135                    0x00, 0x00, 0x00, 0x00, 0x00, 0x44, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00, 0x00,
136                    0xff,
137                ],
138                &[0x44, 0x5f, 0xff, 0x3f, 0x00],
139            ),
140            (
141                &[
142                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
143                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
144                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
145                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
146                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
147                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
148                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
149                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
150                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
151                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
152                    0x83, 0x84, 0x85,
153                ],
154                &[
155                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
156                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
157                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
158                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
159                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
160                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
161                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
162                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
163                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
164                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
165                    0x83, 0x84, 0x85, 0xfe, 0x00,
166                ],
167            ),
168            (
169                &[
170                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
171                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
172                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
173                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
174                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
175                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
176                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
177                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
178                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
179                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
180                    0x83, 0x84, 0x85, 0x00,
181                ],
182                &[
183                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
184                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
185                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
186                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
187                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
188                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
189                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
190                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
191                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
192                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
193                    0x83, 0x84, 0x85, 0xfe, 0x00,
194                ],
195            ),
196            (
197                &[
198                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
199                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
200                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
201                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
202                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
203                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
204                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
205                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
206                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
207                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
208                    0x83, 0x84, 0x85, 0x86,
209                ],
210                &[
211                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
212                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
213                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
214                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
215                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
216                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
217                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
218                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
219                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
220                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
221                    0x83, 0x84, 0x85, 0x86, 0xff, 0x00,
222                ],
223            ),
224            (
225                &[
226                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
227                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
228                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
229                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
230                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
231                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
232                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
233                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
234                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
235                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
236                    0x83, 0x84, 0x85, 0x86, 0x00,
237                ],
238                &[
239                    0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08, 0x09, 0x0a, 0x0b, 0x0c, 0x0d,
240                    0x0e, 0x0f, 0x10, 0x11, 0x12, 0x13, 0x14, 0x15, 0x16, 0x17, 0x18, 0x19, 0x1a,
241                    0x1b, 0x1c, 0x1d, 0x1e, 0x1f, 0x20, 0x21, 0x22, 0x23, 0x24, 0x25, 0x26, 0x27,
242                    0x28, 0x29, 0x2a, 0x2b, 0x2c, 0x2d, 0x2e, 0x2f, 0x30, 0x31, 0x32, 0x33, 0x34,
243                    0x35, 0x36, 0x37, 0x38, 0x39, 0x3a, 0x3b, 0x3c, 0x3d, 0x3e, 0x3f, 0x40, 0x41,
244                    0x42, 0x43, 0x44, 0x45, 0x46, 0x47, 0x48, 0x49, 0x4a, 0x4b, 0x4c, 0x4d, 0x4e,
245                    0x4f, 0x50, 0x51, 0x52, 0x53, 0x54, 0x55, 0x56, 0x57, 0x58, 0x59, 0x5a, 0x5b,
246                    0x5c, 0x5d, 0x5e, 0x5f, 0x60, 0x61, 0x62, 0x63, 0x64, 0x65, 0x66, 0x67, 0x68,
247                    0x69, 0x6a, 0x6b, 0x6c, 0x6d, 0x6e, 0x6f, 0x70, 0x71, 0x72, 0x73, 0x74, 0x75,
248                    0x76, 0x77, 0x78, 0x79, 0x7a, 0x7b, 0x7c, 0x7d, 0x7e, 0x7f, 0x80, 0x81, 0x82,
249                    0x83, 0x84, 0x85, 0x86, 0xff, 0x7f, 0x00,
250                ],
251            ),
252        ];
253
254        for (dec, enc) in tests {
255            let mut res: Vec<u8> = Vec::new();
256
257            let mut e = Encoder::new();
258            e.started = true; // simulate that this is not the first frame.
259
260            e.start_frame(|data| res.extend(data));
261            e.write(dec, |data| res.extend(data));
262            e.end_frame(|data| res.extend(data));
263
264            assert_eq!(enc, &res);
265        }
266    }
267}