bluerobotics_ping/
decoder.rs

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
use tracing::info;

use crate::message::{ProtocolMessage, HEADER};
#[cfg(feature = "serde")]
use serde::{Deserialize, Serialize};

#[derive(Debug, PartialEq, Clone)]
#[cfg_attr(feature = "serde", derive(Serialize, Deserialize))]
pub enum ParseError {
    InvalidStartByte,
    IncompleteData,
    ChecksumError(ProtocolMessage),
}

#[derive(Debug)]
pub enum DecoderResult {
    Success(ProtocolMessage),
    InProgress,
    Error(ParseError),
}

#[derive(Debug)]
pub enum DecoderState {
    AwaitingStart1,
    AwaitingStart2,
    ReadingHeader,
    ReadingPayload,
    ReadingChecksum,
}

pub struct Decoder {
    pub state: DecoderState,
    buffer: Vec<u8>,
    message: ProtocolMessage,
}

impl Decoder {
    pub fn new() -> Self {
        Self {
            state: DecoderState::AwaitingStart1,
            buffer: Vec::new(),
            message: ProtocolMessage::new(),
        }
    }

    pub fn parse_byte(&mut self, byte: u8) -> DecoderResult {
        match self.state {
            DecoderState::AwaitingStart1 => {
                if byte == HEADER[0] {
                    self.state = DecoderState::AwaitingStart2;
                    return DecoderResult::InProgress;
                }
                return DecoderResult::Error(ParseError::InvalidStartByte);
            }
            DecoderState::AwaitingStart2 => {
                if byte == HEADER[1] {
                    self.state = DecoderState::ReadingHeader;
                    self.buffer.clear();
                    return DecoderResult::InProgress;
                }
                self.state = DecoderState::AwaitingStart1;
                return DecoderResult::Error(ParseError::InvalidStartByte);
            }
            DecoderState::ReadingHeader => {
                self.buffer.push(byte);
                // Basic information is available, moving to payload state
                if self.buffer.len() == 6 {
                    self.message.payload_length =
                        u16::from_le_bytes([self.buffer[0], self.buffer[1]]);
                    self.message.message_id = u16::from_le_bytes([self.buffer[2], self.buffer[3]]);
                    self.message.src_device_id = self.buffer[4];
                    self.message.dst_device_id = self.buffer[5];

                    if self.message.payload_length == 0 {
                        self.state = DecoderState::ReadingChecksum
                    } else {
                        self.state = DecoderState::ReadingPayload;
                    }
                    self.buffer.clear();
                }
                return DecoderResult::InProgress;
            }
            DecoderState::ReadingPayload => {
                self.buffer.push(byte);
                info!(
                    "DecoderState : ReadingPayload {:?} {:?}",
                    self.buffer.len(),
                    self.message.payload_length
                );
                if self.buffer.len() == self.message.payload_length as usize {
                    self.message.payload = self.buffer.clone();
                    self.state = DecoderState::ReadingChecksum;
                    self.buffer.clear();
                }
                return DecoderResult::InProgress;
            }
            DecoderState::ReadingChecksum => {
                self.buffer.push(byte);
                if self.buffer.len() == 2 {
                    self.message.checksum = u16::from_le_bytes([self.buffer[0], self.buffer[1]]);
                    self.reset();
                    let message = self.message.clone();
                    self.message = ProtocolMessage::new();
                    if !message.has_valid_crc() {
                        return DecoderResult::Error(ParseError::ChecksumError(message));
                    }
                    return DecoderResult::Success(message);
                }
                return DecoderResult::InProgress;
            }
        }
    }

    fn reset(&mut self) {
        self.state = DecoderState::AwaitingStart1;
        self.buffer.clear();
    }
}