haptic_skin_firmware/
proto.rs

1//! Protocole série binaire host ↔ Pico.
2//!
3//! Format trames (spec complète : `docs/protocol.md`) :
4//! - Host → Pico : `0xAA [cmd] [len] [payload…(len)] [xor]`
5//! - Pico → Host : `0xBB [event] [len] [payload…(len)] [xor]`
6//!
7//! `xor` = OU-exclusif de `[cmd|event, len, payload…]` (tout sauf le start byte
8//! et le xor lui-même).
9
10use crate::config::MOTOR_COUNT;
11
12pub const FRAME_START_H2P: u8 = 0xAA;
13pub const FRAME_START_P2H: u8 = 0xBB;
14
15/// Payload le plus gros = SetAll (8 octets, un par moteur).
16pub const MAX_PAYLOAD: usize = MOTOR_COUNT;
17
18#[repr(u8)]
19#[derive(Copy, Clone, Debug, defmt::Format)]
20pub enum Cmd {
21    Ping = 0x01,
22    SetMotor = 0x10,
23    SetAll = 0x11,
24    StopAll = 0x12,
25}
26
27impl Cmd {
28    fn from_u8(b: u8) -> Option<Self> {
29        Some(match b {
30            0x01 => Cmd::Ping,
31            0x10 => Cmd::SetMotor,
32            0x11 => Cmd::SetAll,
33            0x12 => Cmd::StopAll,
34            _ => return None,
35        })
36    }
37}
38
39#[repr(u8)]
40#[derive(Copy, Clone, Debug, defmt::Format)]
41pub enum Event {
42    Pong = 0x01,
43    Error = 0xFF,
44}
45
46/// Commande décodée + sa charge utile interprétée (ce que la couche moteur reçoit).
47#[derive(Copy, Clone, Debug, defmt::Format, PartialEq)]
48pub enum Command {
49    Ping,
50    SetMotor { idx: u8, intensity: u8 },
51    SetAll([u8; MOTOR_COUNT]),
52    StopAll,
53}
54
55#[derive(Copy, Clone, Debug, defmt::Format, PartialEq)]
56pub enum ProtoError {
57    BadXor,
58    UnknownCmd,
59    BadLength,
60}
61
62impl ProtoError {
63    /// Code d'erreur 1 octet renvoyé dans le payload de l'événement `Error`.
64    pub fn code(self) -> u8 {
65        match self {
66            ProtoError::BadXor => 0x01,
67            ProtoError::UnknownCmd => 0x02,
68            ProtoError::BadLength => 0x03,
69        }
70    }
71}
72
73pub fn xor(buf: &[u8]) -> u8 {
74    buf.iter().fold(0u8, |acc, b| acc ^ b)
75}
76
77/// Décodeur octet-par-octet : les octets USB arrivent en paquets de taille
78/// arbitraire, on accumule jusqu'à une trame complète. Resynchronise tout seul
79/// sur le start byte si on tombe au milieu d'un flux.
80pub struct FrameParser {
81    state: State,
82    cmd: u8,
83    len: u8,
84    payload: heapless::Vec<u8, MAX_PAYLOAD>,
85}
86
87#[derive(Copy, Clone)]
88enum State {
89    WaitStart,
90    WaitCmd,
91    WaitLen,
92    WaitPayload,
93    WaitXor,
94}
95
96impl Default for FrameParser {
97    fn default() -> Self {
98        Self::new()
99    }
100}
101
102impl FrameParser {
103    pub fn new() -> Self {
104        Self {
105            state: State::WaitStart,
106            cmd: 0,
107            len: 0,
108            payload: heapless::Vec::new(),
109        }
110    }
111
112    /// Pousse un octet. Renvoie :
113    /// - `Ok(Some(cmd))` quand une trame complète et valide vient d'être décodée,
114    /// - `Ok(None)` quand il faut encore des octets,
115    /// - `Err(_)` sur trame invalide (le parser se réinitialise alors).
116    pub fn push(&mut self, byte: u8) -> Result<Option<Command>, ProtoError> {
117        match self.state {
118            State::WaitStart => {
119                if byte == FRAME_START_H2P {
120                    self.state = State::WaitCmd;
121                }
122                Ok(None)
123            }
124            State::WaitCmd => {
125                self.cmd = byte;
126                self.state = State::WaitLen;
127                Ok(None)
128            }
129            State::WaitLen => {
130                if byte as usize > MAX_PAYLOAD {
131                    self.reset();
132                    return Err(ProtoError::BadLength);
133                }
134                self.len = byte;
135                self.payload.clear();
136                self.state = if byte == 0 {
137                    State::WaitXor
138                } else {
139                    State::WaitPayload
140                };
141                Ok(None)
142            }
143            State::WaitPayload => {
144                let _ = self.payload.push(byte);
145                if self.payload.len() as u8 == self.len {
146                    self.state = State::WaitXor;
147                }
148                Ok(None)
149            }
150            State::WaitXor => {
151                let mut expected = self.cmd ^ self.len;
152                expected ^= xor(&self.payload);
153                let out = if byte != expected {
154                    Err(ProtoError::BadXor)
155                } else {
156                    self.decode().map(Some)
157                };
158                self.reset();
159                out
160            }
161        }
162    }
163
164    fn decode(&self) -> Result<Command, ProtoError> {
165        let cmd = Cmd::from_u8(self.cmd).ok_or(ProtoError::UnknownCmd)?;
166        Ok(match cmd {
167            Cmd::Ping => {
168                if self.len != 0 {
169                    return Err(ProtoError::BadLength);
170                }
171                Command::Ping
172            }
173            Cmd::StopAll => {
174                if self.len != 0 {
175                    return Err(ProtoError::BadLength);
176                }
177                Command::StopAll
178            }
179            Cmd::SetMotor => {
180                if self.len != 2 {
181                    return Err(ProtoError::BadLength);
182                }
183                Command::SetMotor {
184                    idx: self.payload[0],
185                    intensity: self.payload[1],
186                }
187            }
188            Cmd::SetAll => {
189                if self.len as usize != MOTOR_COUNT {
190                    return Err(ProtoError::BadLength);
191                }
192                let mut arr = [0u8; MOTOR_COUNT];
193                arr.copy_from_slice(&self.payload);
194                Command::SetAll(arr)
195            }
196        })
197    }
198
199    fn reset(&mut self) {
200        self.state = State::WaitStart;
201        self.payload.clear();
202    }
203}
204
205/// Encode une trame Pico → Host dans `out`. Renvoie le nombre d'octets écrits.
206/// `out` doit faire au moins `4 + payload.len()`.
207pub fn encode_event(event: Event, payload: &[u8], out: &mut [u8]) -> usize {
208    let len = payload.len();
209    out[0] = FRAME_START_P2H;
210    out[1] = event as u8;
211    out[2] = len as u8;
212    out[3..3 + len].copy_from_slice(payload);
213    out[3 + len] = (event as u8) ^ (len as u8) ^ xor(payload);
214    4 + len
215}
haptic_skin_firmware/proto.rs

haptic_skin_firmware/
proto.rs
