electricui_embedded/wire/
packet.rs

1use crate::message::{MessageId, MessageType};
2use byteorder::{ByteOrder, LittleEndian};
3use core::fmt;
4use crc::{Algorithm, Crc};
5use err_derive::Error;
6
7#[derive(Debug, Copy, Clone, Eq, PartialEq, Error)]
8pub enum Error {
9    #[error(display = "Not enough bytes for a valid header")]
10    MissingHeader,
11
12    #[error(display = "Not enough bytes for a valid header and checksum")]
13    MissingChecksum,
14
15    #[error(display = "Not enough bytes for a valid payload according to the data length")]
16    IncompletePayload,
17
18    #[error(display = "Invalid checksum")]
19    InvalidChecksum,
20
21    #[error(display = "Invalid message ID length")]
22    InvalidMessageIdLength,
23
24    #[error(display = "Invalid message ID")]
25    InvalidMessageId,
26
27    #[error(display = "Invalid data length")]
28    InvalidDataLength,
29}
30
31#[derive(Debug, Clone)]
32pub struct Packet<T: AsRef<[u8]>> {
33    buffer: T,
34}
35
36mod field {
37    use crate::wire::{Field, Rest};
38
39    // Header field byte indices and ranges
40    pub const DATA_LEN: Field = 0..2;
41    pub const TYPE: usize = 1;
42    pub const INTERNAL: usize = 1;
43    pub const OFFSET: usize = 1;
44    pub const ID_LEN: usize = 2;
45    pub const RESPONSE: usize = 2;
46    pub const ACKNUM: usize = 2;
47
48    // Message ID bytes followed by maybe offset and maybe packet payload
49    pub const REST: Rest = 3..;
50    // Followed by 2 byte checksum
51}
52
53impl<T: AsRef<[u8]>> Packet<T> {
54    pub const HEADER_SIZE: usize = 3;
55    pub const CHECKSUM_SIZE: usize = 2;
56    pub const OFFSET_SIZE: usize = 2;
57    pub const MAX_PAYLOAD_SIZE: usize = 1024;
58    pub const MAX_MSG_ID_SIZE: usize = 15;
59
60    pub const BASE_PACKET_SIZE: usize = Self::HEADER_SIZE + Self::CHECKSUM_SIZE;
61
62    pub const MAX_PACKET_SIZE: usize =
63        Self::BASE_PACKET_SIZE + Self::MAX_MSG_ID_SIZE + Self::MAX_PAYLOAD_SIZE;
64
65    pub const CRC16_CCITT_FALSE: Algorithm<u16> = Algorithm {
66        poly: 0x1021,
67        init: 0xFFFF,
68        refin: false,
69        refout: false,
70        xorout: 0,
71        check: 0x29B1,
72        residue: 0,
73    };
74
75    pub fn new_unchecked(buffer: T) -> Packet<T> {
76        Packet { buffer }
77    }
78
79    pub fn new(buffer: T) -> Result<Packet<T>, Error> {
80        let p = Self::new_unchecked(buffer);
81        p.check_len()?;
82        p.check_payload_length()?;
83        p.check_checksum()?;
84        Ok(p)
85    }
86
87    pub fn check_len(&self) -> Result<(), Error> {
88        let len = self.buffer.as_ref().len();
89        if len < field::REST.start {
90            Err(Error::MissingHeader)
91        } else if len < (field::REST.start + Self::CHECKSUM_SIZE) {
92            Err(Error::MissingChecksum)
93        } else {
94            Ok(())
95        }
96    }
97
98    /// Checks that the buffer contains enough bytes to read
99    /// both the message ID and the payload bytes
100    pub fn check_payload_length(&self) -> Result<(), Error> {
101        let id_len = self.id_length()?;
102        let data_len = usize::from(self.data_length());
103        let len = self.buffer.as_ref().len();
104        if len < Self::buffer_len(id_len, data_len) {
105            Err(Error::IncompletePayload)
106        } else {
107            Ok(())
108        }
109    }
110
111    pub fn check_checksum(&self) -> Result<(), Error> {
112        let provided = self.checksum()?;
113        let computed = self.compute_checksum()?;
114        if computed != provided {
115            Err(Error::InvalidChecksum)
116        } else {
117            Ok(())
118        }
119    }
120
121    #[inline]
122    pub fn wire_size(&self) -> Result<usize, Error> {
123        let id_len = self.id_length()?;
124        let data_len = usize::from(self.data_length());
125        Ok(Self::buffer_len(id_len, data_len))
126    }
127
128    pub fn into_inner(self) -> T {
129        self.buffer
130    }
131
132    /// Return the length of a buffer required to hold a message
133    /// with a payload length of `n_msg_id_bytes` + `n_payload_bytes`.
134    #[inline]
135    pub fn buffer_len(n_msg_id_bytes: usize, n_payload_bytes: usize) -> usize {
136        Self::BASE_PACKET_SIZE + n_msg_id_bytes + n_payload_bytes
137    }
138
139    #[inline]
140    pub fn data_length(&self) -> u16 {
141        let data = self.buffer.as_ref();
142        LittleEndian::read_u16(&data[field::DATA_LEN]) & 0x3FF
143    }
144
145    #[inline]
146    pub fn typ_raw(&self) -> u8 {
147        let data = self.buffer.as_ref();
148        (data[field::TYPE] >> 2) & 0x0F
149    }
150
151    #[inline]
152    pub fn typ(&self) -> MessageType {
153        let typ = self.typ_raw();
154        MessageType::from(typ)
155    }
156
157    #[inline]
158    pub fn internal(&self) -> bool {
159        let data = self.buffer.as_ref();
160        ((data[field::INTERNAL] >> 6) & 0x01) != 0
161    }
162
163    #[inline]
164    pub fn offset(&self) -> bool {
165        let data = self.buffer.as_ref();
166        ((data[field::OFFSET] >> 7) & 0x01) != 0
167    }
168
169    #[inline]
170    pub fn id_length_raw(&self) -> u8 {
171        let data = self.buffer.as_ref();
172        data[field::ID_LEN] & 0x0F
173    }
174
175    #[inline]
176    pub fn id_length(&self) -> Result<usize, Error> {
177        let id = self.id_length_raw();
178        if id == 0 {
179            Err(Error::InvalidMessageIdLength)
180        } else {
181            Ok(id.into())
182        }
183    }
184
185    #[inline]
186    pub fn response(&self) -> bool {
187        let data = self.buffer.as_ref();
188        ((data[field::RESPONSE] >> 4) & 0x01) != 0
189    }
190
191    #[inline]
192    pub fn acknum(&self) -> u8 {
193        let data = self.buffer.as_ref();
194        (data[field::ACKNUM] >> 5) & 0x07
195    }
196
197    #[inline]
198    pub fn checksum(&self) -> Result<u16, Error> {
199        let id_len = self.id_length()?;
200        let data_len = usize::from(self.data_length());
201        let start = field::REST.start + id_len + data_len;
202        let end = start + Self::CHECKSUM_SIZE;
203        let data = self.buffer.as_ref();
204        debug_assert!(end <= data.len());
205        Ok(LittleEndian::read_u16(&data[start..end]))
206    }
207
208    #[inline]
209    pub fn compute_checksum(&self) -> Result<u16, Error> {
210        let crc = Crc::<u16>::new(&Self::CRC16_CCITT_FALSE);
211        let id_len = self.id_length()?;
212        let data_len = usize::from(self.data_length());
213        let end = Self::HEADER_SIZE + id_len + data_len;
214        let data = self.buffer.as_ref();
215        debug_assert!(end <= data.len());
216        Ok(crc.checksum(&data[..end]))
217    }
218}
219
220impl<T: AsRef<[u8]>> Packet<T> {
221    #[inline]
222    pub fn msg_id_raw(&self) -> Result<&[u8], Error> {
223        let id_len = self.id_length()?;
224        let end = field::REST.start + id_len;
225        let data = self.buffer.as_ref();
226        debug_assert!(end <= data.len());
227        Ok(&data[field::REST.start..end])
228    }
229
230    #[inline]
231    pub fn msg_id(&self) -> Result<MessageId<'_>, Error> {
232        let msg_id = self.msg_id_raw()?;
233        MessageId::new(msg_id).ok_or(Error::InvalidMessageId)
234    }
235
236    #[inline]
237    pub fn payload(&self) -> Result<&[u8], Error> {
238        let id_len = self.id_length()?;
239        let data_len = usize::from(self.data_length());
240        let start = field::REST.start + id_len;
241        let end = start + data_len;
242        let data = self.buffer.as_ref();
243        debug_assert!(end <= data.len());
244        Ok(&data[start..end])
245    }
246}
247
248impl<T: AsRef<[u8]> + AsMut<[u8]>> Packet<T> {
249    #[inline]
250    pub fn set_data_length(&mut self, value: u16) -> Result<(), Error> {
251        if usize::from(value) > Self::MAX_PAYLOAD_SIZE {
252            Err(Error::InvalidDataLength)
253        } else {
254            let data = self.buffer.as_mut();
255            LittleEndian::write_u16(&mut data[field::DATA_LEN], value & 0x3FF);
256            Ok(())
257        }
258    }
259
260    #[inline]
261    pub fn set_typ(&mut self, value: MessageType) {
262        let data = self.buffer.as_mut();
263        data[field::TYPE] = (data[field::TYPE] & !0x3C) | (u8::from(value) << 2);
264    }
265
266    #[inline]
267    pub fn set_internal(&mut self, value: bool) {
268        let data = self.buffer.as_mut();
269        if value {
270            data[field::INTERNAL] |= 1 << 6;
271        } else {
272            data[field::INTERNAL] &= !(1 << 6);
273        }
274    }
275
276    #[inline]
277    pub fn set_offset(&mut self, value: bool) {
278        let data = self.buffer.as_mut();
279        if value {
280            data[field::OFFSET] |= 1 << 7;
281        } else {
282            data[field::OFFSET] &= !(1 << 7);
283        }
284    }
285
286    #[inline]
287    pub fn set_id_length(&mut self, value: u8) -> Result<(), Error> {
288        if value == 0 || usize::from(value) > Self::MAX_MSG_ID_SIZE {
289            Err(Error::InvalidMessageIdLength)
290        } else {
291            let data = self.buffer.as_mut();
292            data[field::ID_LEN] = (data[field::ID_LEN] & !0x0F) | (value & 0x0F);
293            Ok(())
294        }
295    }
296
297    #[inline]
298    pub fn set_response(&mut self, value: bool) {
299        let data = self.buffer.as_mut();
300        if value {
301            data[field::RESPONSE] |= 1 << 4;
302        } else {
303            data[field::RESPONSE] &= !(1 << 4);
304        }
305    }
306
307    #[inline]
308    pub fn set_acknum(&mut self, value: u8) {
309        let data = self.buffer.as_mut();
310        data[field::ACKNUM] = (data[field::ACKNUM] & !0xE0) | ((value & 0x07) << 5);
311    }
312
313    #[inline]
314    pub fn msg_id_mut(&mut self) -> Result<&mut [u8], Error> {
315        let id_len = self.id_length()?;
316        let end = field::REST.start + id_len;
317        let data = self.buffer.as_mut();
318        debug_assert!(end <= data.len());
319        Ok(&mut data[field::REST.start..end])
320    }
321
322    #[inline]
323    pub fn payload_mut(&mut self) -> Result<&mut [u8], Error> {
324        let id_len = self.id_length()?;
325        let data_len = usize::from(self.data_length());
326        let start = field::REST.start + id_len;
327        let end = start + data_len;
328        let data = self.buffer.as_mut();
329        debug_assert!(end <= data.len());
330        Ok(&mut data[start..end])
331    }
332
333    #[inline]
334    pub fn set_checksum(&mut self, value: u16) -> Result<(), Error> {
335        let id_len = self.id_length()?;
336        let data_len = usize::from(self.data_length());
337        let start = field::REST.start + id_len + data_len;
338        let end = start + Self::CHECKSUM_SIZE;
339        let data = self.buffer.as_mut();
340        debug_assert!(end <= data.len());
341        LittleEndian::write_u16(&mut data[start..end], value);
342        Ok(())
343    }
344}
345
346impl<T: AsRef<[u8]>> AsRef<[u8]> for Packet<T> {
347    fn as_ref(&self) -> &[u8] {
348        self.buffer.as_ref()
349    }
350}
351
352impl<T: AsRef<[u8]>> fmt::Display for Packet<T> {
353    fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
354        write!(
355            f,
356            "{{ DataLen({}), Type({}), Int({}), Offset({}), IdLen({}), Resp({}), Acknum({}) }}",
357            self.data_length(),
358            self.typ_raw(),
359            self.internal() as u8,
360            self.offset() as u8,
361            self.id_length_raw(),
362            self.response() as u8,
363            self.acknum()
364        )
365    }
366}
367
368#[cfg(test)]
369mod tests {
370    use super::*;
371    use crate::wire::framing::Framing;
372    use approx::assert_relative_eq;
373    use pretty_assertions::assert_eq;
374
375    static MSG_I8: [u8; 9 + 2] = [
376        0x0A, // framing
377        0x01, 0x14, 0x63, // header
378        0x61, 0x62, 0x63, // msgid
379        0x2A, // payload
380        0xB8, 0xA3, // crc
381        0x00, // framing
382    ];
383
384    static MSG_F32: [u8; 12 + 2] = [
385        0x0D, // framing
386        0x04, 0x2c, 0x03, // header
387        0x61, 0x62, 0x63, // msgid
388        0x14, 0xAE, 0x29, 0x42, // payload
389        0x8B, 0x1D, // crc
390        0x00, // framing
391    ];
392
393    #[test]
394    fn construct_i8() {
395        let mut bytes = [0xFF; 9];
396        let mut p = Packet::new_unchecked(&mut bytes[..]);
397        assert!(p.check_len().is_ok());
398        p.set_data_length(1).unwrap();
399        p.set_typ(MessageType::I8);
400        p.set_internal(false);
401        p.set_offset(false);
402        p.set_id_length(3).unwrap();
403        p.set_response(false);
404        p.set_acknum(3);
405        p.msg_id_mut().unwrap().copy_from_slice(b"abc");
406        p.payload_mut().unwrap()[0] = 0x2A;
407        p.set_checksum(0xA3B8).unwrap();
408        assert!(p.check_payload_length().is_ok());
409        assert!(p.check_checksum().is_ok());
410        assert_eq!(p.wire_size(), Ok(9));
411        assert_eq!(&p.into_inner()[..], &MSG_I8[1..10]);
412
413        let mut enc_bytes = [0xFF; 9 + 2];
414        assert!(enc_bytes.len() == Framing::max_encoded_len(9));
415        let size = Framing::encode_buf(&bytes[..], &mut enc_bytes[..]);
416        assert_eq!(size, 9 + 2);
417        assert_eq!(&enc_bytes[..], &MSG_I8[..]);
418    }
419
420    #[test]
421    fn deconstruct_i8() {
422        let mut bytes = [0xFF; 9];
423        let size = Framing::decode_buf(&MSG_I8[..], &mut bytes[..]).unwrap();
424        assert_eq!(size, bytes.len());
425
426        assert_eq!(Packet::<&[u8]>::buffer_len(3, 1), bytes.len());
427        let p = Packet::new(&bytes[..]).unwrap();
428        assert_eq!(p.data_length(), 1);
429        assert_eq!(p.typ(), MessageType::I8);
430        assert_eq!(p.internal(), false);
431        assert_eq!(p.offset(), false);
432        assert_eq!(p.id_length().unwrap(), 3);
433        assert_eq!(p.response(), false);
434        assert_eq!(p.acknum(), 3);
435        assert_eq!(p.msg_id().unwrap(), b"abc");
436        assert_eq!(p.payload().unwrap(), &[0x2A]);
437        assert_eq!(p.checksum().unwrap(), 0xA3B8);
438        assert_eq!(p.compute_checksum().unwrap(), 0xA3B8);
439        assert_eq!(p.wire_size(), Ok(9));
440    }
441
442    #[test]
443    fn construct_f32() {
444        let mut bytes = [0xFF; 12];
445        let mut p = Packet::new_unchecked(&mut bytes[..]);
446        assert!(p.check_len().is_ok());
447        p.set_data_length(4).unwrap();
448        p.set_typ(MessageType::F32);
449        p.set_internal(false);
450        p.set_offset(false);
451        p.set_id_length(3).unwrap();
452        p.set_response(false);
453        p.set_acknum(0);
454        p.msg_id_mut().unwrap().copy_from_slice(b"abc");
455        LittleEndian::write_f32(p.payload_mut().unwrap(), 42.42_f32);
456        p.set_checksum(0x1D8B).unwrap();
457        assert!(p.check_payload_length().is_ok());
458        assert!(p.check_checksum().is_ok());
459        assert_eq!(p.wire_size(), Ok(12));
460        assert_eq!(&p.into_inner()[..], &MSG_F32[1..13]);
461
462        let mut enc_bytes = [0xFF; 12 + 2];
463        assert!(enc_bytes.len() == Framing::max_encoded_len(12));
464        let size = Framing::encode_buf(&bytes[..], &mut enc_bytes[..]);
465        assert_eq!(size, 12 + 2);
466        assert_eq!(&enc_bytes[..], &MSG_F32[..]);
467    }
468
469    #[test]
470    fn deconstruct_f32() {
471        let mut bytes = [0xFF; 12];
472        let size = Framing::decode_buf(&MSG_F32[..], &mut bytes[..]).unwrap();
473        assert_eq!(size, bytes.len());
474
475        assert_eq!(Packet::<&[u8]>::buffer_len(3, 4), bytes.len());
476        let p = Packet::new(&bytes[..]).unwrap();
477        assert_eq!(p.data_length(), 4);
478        assert_eq!(p.typ(), MessageType::F32);
479        assert_eq!(p.internal(), false);
480        assert_eq!(p.offset(), false);
481        assert_eq!(p.id_length().unwrap(), 3);
482        assert_eq!(p.response(), false);
483        assert_eq!(p.acknum(), 0);
484        assert_eq!(p.msg_id().unwrap(), b"abc");
485        assert_eq!(p.payload().unwrap(), &[0x14, 0xAE, 0x29, 0x42]);
486        assert_relative_eq!(LittleEndian::read_f32(p.payload().unwrap()), 42.42_f32);
487        assert_eq!(p.checksum().unwrap(), 0x1D8B);
488        assert_eq!(p.compute_checksum().unwrap(), 0x1D8B);
489        assert_eq!(p.wire_size(), Ok(12));
490    }
491
492    #[test]
493    fn buffer_len() {
494        assert_eq!(
495            Packet::<&[u8]>::buffer_len(1, 0),
496            Packet::<&[u8]>::BASE_PACKET_SIZE + 1
497        );
498        assert_eq!(
499            Packet::<&[u8]>::buffer_len(3, 4),
500            Packet::<&[u8]>::BASE_PACKET_SIZE + 3 + 4
501        );
502    }
503
504    #[test]
505    fn missing_header() {
506        let bytes = [0xFF; 5 - 3];
507        assert_eq!(bytes.len(), Packet::<&[u8]>::buffer_len(0, 0) - 3);
508        let p = Packet::new(&bytes[..]);
509        assert_eq!(p.unwrap_err(), Error::MissingHeader);
510    }
511
512    #[test]
513    fn missing_checksum() {
514        let bytes = [0xFF; 5 - 1];
515        assert_eq!(bytes.len(), Packet::<&[u8]>::buffer_len(0, 0) - 1);
516        let p = Packet::new(&bytes[..]);
517        assert_eq!(p.unwrap_err(), Error::MissingChecksum);
518    }
519
520    #[test]
521    fn incomplete_payload() {
522        let bytes = [0x04, 0x2c, 0x03, 0xFF, 0xFF];
523        let p = Packet::new(&bytes[..]);
524        assert_eq!(p.unwrap_err(), Error::IncompletePayload);
525    }
526
527    #[test]
528    fn invalid_checksum() {
529        let bytes = [0x01, 0x14, 0x63, 0x61, 0x62, 0x63, 0x2A, 0xB8, 0xA3 + 1];
530        let p = Packet::new(&bytes[..]);
531        assert_eq!(p.unwrap_err(), Error::InvalidChecksum);
532    }
533
534    #[test]
535    fn invalid_msg_id_len() {
536        let mut bytes = [0x01, 0x14, 0x63, 0x61, 0x62, 0x63, 0x2A, 0xB8, 0xA3];
537        let mut p = Packet::new(&mut bytes[..]).unwrap();
538        assert_eq!(
539            p.set_id_length(0).unwrap_err(),
540            Error::InvalidMessageIdLength
541        );
542        assert_eq!(
543            p.set_id_length(Packet::<&[u8]>::MAX_MSG_ID_SIZE as u8 + 1)
544                .unwrap_err(),
545            Error::InvalidMessageIdLength
546        );
547        bytes[field::ID_LEN] &= !0x0F; // zero
548        let p = Packet::new(&bytes[..]);
549        assert_eq!(p.unwrap_err(), Error::InvalidMessageIdLength);
550    }
551
552    #[test]
553    fn invalid_msg_id() {
554        let mut bytes = [0xFF; 7];
555        let mut p = Packet::new_unchecked(&mut bytes[..]);
556        assert!(p.check_len().is_ok());
557        p.set_data_length(0).unwrap();
558        p.set_typ(MessageType::Custom);
559        p.set_internal(false);
560        p.set_offset(false);
561        p.set_id_length(1).unwrap();
562        p.set_response(false);
563        p.set_acknum(0);
564        p.msg_id_mut().unwrap().copy_from_slice(&[0]); // zero invalid
565        p.set_checksum(p.compute_checksum().unwrap()).unwrap();
566        assert!(p.check_payload_length().is_ok());
567        assert!(p.check_checksum().is_ok());
568
569        let p = Packet::new(&bytes[..]).unwrap();
570        assert_eq!(p.msg_id().unwrap_err(), Error::InvalidMessageId);
571    }
572
573    #[test]
574    fn invalid_data_len() {
575        let mut bytes = [0xFF; 32];
576        let mut p = Packet::new_unchecked(&mut bytes[..]);
577        assert!(p.check_len().is_ok());
578        assert_eq!(
579            p.set_data_length(Packet::<&[u8]>::MAX_PAYLOAD_SIZE as u16 + 1)
580                .unwrap_err(),
581            Error::InvalidDataLength
582        );
583    }
584
585    #[test]
586    fn unknown_msg_type() {
587        let mut bytes = [0x01, 0x14, 0x63, 0x61, 0x62, 0x63, 0x2A, 0xB8, 0xA3];
588        bytes[field::TYPE] = (bytes[field::TYPE] & !0x3C) | (0x0F << 2);
589        let p = Packet::new_unchecked(&mut bytes[..]);
590        assert_eq!(p.typ(), MessageType::Unknown(0x0F));
591    }
592}
electricui_embedded/wire/packet.rs

electricui_embedded/wire/
packet.rs
