shproto_rs/
lib.rs

1#![no_std]
2
3use heapless;
4
5pub fn crc16(crc: u16, byte: u8) -> u16 {
6    let mut crc = crc ^ (byte as u16);
7    for _ in 0..8 {
8        if (crc & 0x0001) != 0 {
9            crc = (crc >> 1) ^ 0xA001
10        } else {
11            crc = crc >> 1
12        }
13    }
14    crc
15}
16
17#[derive(Debug, Copy, Clone)]
18pub enum ShprotoError {
19    PushFailed
20}
21
22pub enum ControlByte {}
23impl ControlByte {
24    pub const START: u8 = 0xFE;
25    pub const ESCAPE: u8 = 0xFD;
26    pub const STOP: u8 = 0xA5;
27}
28
29#[derive(Debug)]
30pub struct ShprotoPacket<const N: usize = 256> {
31    pub data: heapless::Vec<u8, N>,
32    crc: u16,
33    completed: bool,
34    valid: bool,
35}
36impl<const N: usize> ShprotoPacket<N> {
37    pub fn new() -> Self {
38        let mut p = ShprotoPacket {
39            data: Default::default(),
40            crc: 0xFFFF,
41            completed: false,
42            valid: false
43        };
44        p.data.push(0xFF).unwrap();
45        p.data.push(0xFE).unwrap();
46        p
47    }
48
49    pub fn start(&mut self, command: u8) -> Result<(), ShprotoError> {
50        self.add_byte(command)
51    }
52
53    pub fn add_byte(&mut self, byte: u8) -> Result<(), ShprotoError>{
54        // calculate crc
55        self.crc = crc16(self.crc, byte);
56        // push byte
57        let need_escape: bool = match byte {
58            ControlByte::START | ControlByte::ESCAPE | ControlByte::STOP => true,
59            _ => false
60        };
61        if need_escape {
62            self.data.push(ControlByte::ESCAPE)
63                .map_err(|_| ShprotoError::PushFailed)?;
64            self.data.push((!byte) & 0xFF)
65                .map_err(|_| ShprotoError::PushFailed)?;
66        } else {
67            self.data.push(byte)
68                .map_err(|_| ShprotoError::PushFailed)?;
69        }
70        Ok(())
71    }
72
73    pub fn complete(&mut self) {
74        // get CRC bytes
75        for byte in self.crc.to_le_bytes().iter() {
76            self.add_byte(*byte).unwrap()
77        }
78        self.data.push(ControlByte::STOP).unwrap();
79        self.completed = true;
80        if self.crc == 0 {
81            self.valid = true;
82        }
83    }
84}
85
86enum ShprotoParserState {
87    Start,
88    Data,
89    EscapedData,
90}
91
92pub struct ShprotoParser<const N: usize> {
93    state: ShprotoParserState,
94    packet: ShprotoPacket<N>,
95}
96
97impl<const N: usize> ShprotoParser<N> {
98    pub fn new() -> Self {
99        ShprotoParser {
100            state: ShprotoParserState::Start,
101            packet: ShprotoPacket::new(),
102        }
103    }
104
105    pub fn parse_byte(&mut self, byte: u8) -> Result<Option<ShprotoPacket<N>>, ShprotoError> {
106        match self.state {
107            ShprotoParserState::Start => {
108                if byte == ControlByte::START {
109                    self.packet = ShprotoPacket::new();
110                    self.state = ShprotoParserState::Data;
111                }
112            }
113            ShprotoParserState::Data => {
114                match byte {
115                    ControlByte::START => {
116                        self.packet = ShprotoPacket::new();
117                        self.state = ShprotoParserState::Data;
118                    }
119                    ControlByte::ESCAPE => {
120                        self.state = ShprotoParserState::EscapedData;
121                    }
122                    ControlByte::STOP => {
123                        // Create a new packet to return.
124                        let completed_packet = ShprotoPacket {
125                            data: self.packet.data.clone(),
126                            crc: self.packet.crc,
127                            completed: true,
128                            valid: self.packet.crc == 0,
129                        };
130
131                        // Reset the parser state and return the completed packet.
132                        self.state = ShprotoParserState::Start;
133                        self.packet = ShprotoPacket::new();
134                        return Ok(Some(completed_packet));
135                    }
136                    _ => {
137                        self.packet.add_byte(byte)?;
138                    }
139                }
140            }
141            ShprotoParserState::EscapedData => {
142                let unescaped_byte = (!byte) & 0xFF;
143                self.packet.add_byte(unescaped_byte)?;
144                self.state = ShprotoParserState::Data;
145            }
146        }
147
148        Ok(None)
149    }
150}
151
152#[cfg(test)]
153mod tests {
154    use super::*;
155
156    #[test]
157    fn build() {
158        let mut packet = ShprotoPacket::<256>::new();
159        packet.start(0x03).unwrap();
160        packet.add_byte(0x99).unwrap();
161        assert_eq!(packet.crc, 10945);
162        packet.complete();
163        assert_eq!(packet.crc, 0);
164        assert_eq!(packet.completed, true);
165        assert_eq!(packet.valid, true);
166    }
167    #[test]
168    fn parse() {
169        let bytes = [0xFF, 0xFE, 0x69, 0x8C, 0x90, 0x8C, 0x89, 0xFD, 0x5A, 0x53, 0xFD, 0x02, 0xA5];
170        let mut parser = ShprotoParser::<4096>::new();
171        let mut packet_counter: u32 = 0;
172        for byte in bytes.as_slice() {
173            if let Some(packet) = parser.parse_byte(*byte).unwrap() {
174                assert_eq!(packet.crc, 0);
175                packet_counter += 1;
176            }
177        }
178        assert_eq!(packet_counter, 1);
179    }
180}
shproto_rs/lib.rs

shproto_rs/
lib.rs
