cu_msp_bridge/
lib.rs

1//! Copper bridge that multiplexes MSP traffic in both directions over a single serial port.
2//! The type exposes a `requests` Tx channel that accepts [`MspRequestBatch`] messages and a
3//! `responses` Rx channel that yields [`MspResponseBatch`] payloads decoded from the line.
4
5#![cfg_attr(not(feature = "std"), no_std)]
6
7#[cfg(not(feature = "std"))]
8extern crate alloc;
9
10// std implementation
11#[cfg(feature = "std")]
12mod std_impl {
13    pub use std::{format, mem, string::String, vec::Vec};
14
15    pub const DEVICE_KEY: &str = "device";
16    pub const BAUD_KEY: &str = "baudrate";
17    pub const TIMEOUT_KEY: &str = "timeout_ms";
18    pub const DEFAULT_DEVICE: &str = "/dev/ttyUSB0";
19    pub const DEFAULT_BAUDRATE: u32 = 115_200;
20    pub const DEFAULT_TIMEOUT_MS: u64 = 50;
21}
22
23// no-std implementation
24#[cfg(not(feature = "std"))]
25mod no_std_impl {
26    pub use alloc::{format, vec::Vec};
27    pub use core::mem;
28
29    pub const SERIAL_INDEX_KEY: &str = "serial_port_index";
30}
31
32#[cfg(not(feature = "std"))]
33use no_std_impl::*;
34#[cfg(feature = "std")]
35use std_impl::*;
36
37use bincode::de::Decoder;
38use bincode::enc::Encoder;
39use bincode::error::{DecodeError, EncodeError};
40use bincode::{Decode, Encode};
41use cu29::cubridge::{
42    BridgeChannel, BridgeChannelConfig, BridgeChannelInfo, BridgeChannelSet, CuBridge,
43};
44use cu29::prelude::*;
45use cu_msp_lib::structs::{MspRequest, MspResponse};
46use cu_msp_lib::{MspPacket, MspParser};
47use embedded_io::{Read, Write};
48use serde::{Deserialize, Serialize};
49use smallvec::SmallVec;
50
51const READ_BUFFER_SIZE: usize = 512;
52const MAX_REQUESTS_PER_BATCH: usize = 8;
53const MAX_RESPONSES_PER_BATCH: usize = 16;
54
55pub trait SerialFactory<E>: Write<Error = E> + Read<Error = E> + Send + 'static {
56    fn try_new(config: Option<&ComponentConfig>) -> CuResult<Self>
57    where
58        Self: Sized;
59}
60
61#[cfg(not(feature = "std"))]
62impl<S, E> SerialFactory<E> for S
63where
64    S: Write<Error = E> + Read<Error = E> + Send + 'static,
65{
66    fn try_new(config: Option<&ComponentConfig>) -> CuResult<Self> {
67        let slot = config
68            .and_then(|cfg| cfg.get::<u32>(SERIAL_INDEX_KEY))
69            .map(|v| v as usize)
70            .unwrap_or(0);
71
72        cu_embedded_registry::take(slot).ok_or_else(|| {
73            CuError::from(format!(
74                "MSP bridge missing serial for slot {slot} (max index {})",
75                cu_embedded_registry::MAX_SERIAL_SLOTS - 1
76            ))
77        })
78    }
79}
80
81#[cfg(feature = "std")]
82impl SerialFactory<std::io::Error> for std_serial::StdSerial {
83    fn try_new(config: Option<&ComponentConfig>) -> CuResult<Self> {
84        let device = config
85            .and_then(|cfg| cfg.get::<String>(DEVICE_KEY))
86            .filter(|s| !s.is_empty())
87            .unwrap_or_else(|| DEFAULT_DEVICE.to_string());
88
89        let baudrate = config
90            .and_then(|cfg| cfg.get::<u32>(BAUD_KEY))
91            .unwrap_or(DEFAULT_BAUDRATE);
92
93        let timeout_ms = config
94            .and_then(|cfg| cfg.get::<u64>(TIMEOUT_KEY))
95            .unwrap_or(DEFAULT_TIMEOUT_MS);
96
97        std_serial::open(&device, baudrate, timeout_ms).map_err(|err| {
98            CuError::from(format!(
99                "MSP bridge failed to open serial `{device}` at {baudrate} baud: {err}"
100            ))
101        })
102    }
103}
104
105/// Batch of MSP requests transported over the bridge.
106#[derive(Debug, Clone, Default, Serialize, Deserialize)]
107pub struct MspRequestBatch(pub SmallVec<[MspRequest; MAX_REQUESTS_PER_BATCH]>);
108
109impl MspRequestBatch {
110    pub fn new() -> Self {
111        Self(SmallVec::new())
112    }
113
114    pub fn push(&mut self, req: MspRequest) {
115        let Self(ref mut vec) = self;
116        vec.push(req);
117    }
118
119    pub fn iter(&self) -> impl Iterator<Item = &MspRequest> {
120        let Self(ref vec) = self;
121        vec.iter()
122    }
123}
124
125impl Encode for MspRequestBatch {
126    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
127        Encode::encode(&self.0.as_slice(), encoder)
128    }
129}
130
131impl Decode<()> for MspRequestBatch {
132    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
133        let values = <Vec<MspRequest> as Decode<()>>::decode(decoder)?;
134        Ok(Self(values.into()))
135    }
136}
137
138/// Batch of MSP responses collected by the bridge.
139#[derive(Debug, Clone, Default, Serialize, Deserialize)]
140pub struct MspResponseBatch(pub SmallVec<[MspResponse; MAX_RESPONSES_PER_BATCH]>);
141
142impl MspResponseBatch {
143    pub fn new() -> Self {
144        Self(SmallVec::new())
145    }
146
147    pub fn push(&mut self, resp: MspResponse) {
148        let Self(ref mut vec) = self;
149        vec.push(resp);
150    }
151
152    pub fn clear(&mut self) {
153        self.0.clear();
154    }
155
156    pub fn is_empty(&self) -> bool {
157        self.0.is_empty()
158    }
159}
160
161impl Encode for MspResponseBatch {
162    fn encode<E: Encoder>(&self, encoder: &mut E) -> Result<(), EncodeError> {
163        Encode::encode(&self.0.as_slice(), encoder)
164    }
165}
166
167impl Decode<()> for MspResponseBatch {
168    fn decode<D: Decoder<Context = ()>>(decoder: &mut D) -> Result<Self, DecodeError> {
169        let values = <Vec<MspResponse> as Decode<()>>::decode(decoder)?;
170        Ok(Self(values.into()))
171    }
172}
173
174tx_channels! {
175    pub struct TxChannels : TxId {
176        requests => MspRequestBatch,
177    }
178}
179
180rx_channels! {
181    pub struct RxChannels : RxId {
182        responses => MspResponseBatch,
183    }
184}
185
186/// Bridge that multiplexes MSP traffic on a single serial link.
187pub struct CuMspBridge<S, E>
188where
189    S: Write<Error = E> + Read<Error = E>,
190{
191    serial: S,
192    parser: MspParser,
193    read_buffer: [u8; READ_BUFFER_SIZE],
194    pending_responses: MspResponseBatch,
195    tx_buffer: SmallVec<[u8; 256]>,
196}
197
198impl<S, E> CuMspBridge<S, E>
199where
200    S: Write<Error = E> + Read<Error = E>,
201{
202    fn from_serial(serial: S) -> Self {
203        Self {
204            serial,
205            parser: MspParser::new(),
206            read_buffer: [0; READ_BUFFER_SIZE],
207            pending_responses: MspResponseBatch::new(),
208            tx_buffer: SmallVec::new(),
209        }
210    }
211
212    fn send_request(&mut self, request: &MspRequest) -> CuResult<()> {
213        let packet: MspPacket = request.into();
214        let size = packet.packet_size_bytes();
215        self.tx_buffer.resize(size, 0);
216        packet.serialize(&mut self.tx_buffer).map_err(|err| {
217            CuError::new_with_cause("MSP bridge failed to serialize request", err)
218        })?;
219        self.serial
220            .write_all(&self.tx_buffer)
221            .map_err(|_| CuError::from("MSP bridge failed to write serial"))
222    }
223
224    fn poll_serial(&mut self) -> CuResult<()> {
225        loop {
226            match self.serial.read(&mut self.read_buffer) {
227                Ok(0) => break,
228                Ok(n) => {
229                    for &byte in &self.read_buffer[..n] {
230                        match self.parser.parse(byte) {
231                            Ok(Some(packet)) => {
232                                let response = MspResponse::from(packet);
233                                self.pending_responses.push(response);
234                                if self.pending_responses.0.len() >= MAX_RESPONSES_PER_BATCH {
235                                    break;
236                                }
237                            }
238                            Ok(None) => {}
239                            Err(err) => {
240                                error!("MSP bridge parser error: {}", err.to_string());
241                            }
242                        }
243                    }
244                }
245                Err(_) => break,
246            }
247        }
248        Ok(())
249    }
250}
251
252#[cfg(not(feature = "std"))]
253impl<S, E> CuMspBridge<S, E>
254where
255    S: SerialFactory<E>,
256{
257    /// Register a serial port for use with MSP bridge in no-std environments
258    pub fn register_serial(slot: usize, serial_port: S) -> CuResult<()> {
259        cu_embedded_registry::register(slot, serial_port)
260    }
261}
262
263impl<S, E> Freezable for CuMspBridge<S, E> where S: Write<Error = E> + Read<Error = E> {}
264
265impl<S, E> CuBridge for CuMspBridge<S, E>
266where
267    S: SerialFactory<E>,
268{
269    type Tx = TxChannels;
270    type Rx = RxChannels;
271
272    fn new(
273        config: Option<&ComponentConfig>,
274        tx_channels: &[BridgeChannelConfig<<Self::Tx as BridgeChannelSet>::Id>],
275        rx_channels: &[BridgeChannelConfig<<Self::Rx as BridgeChannelSet>::Id>],
276    ) -> CuResult<Self>
277    where
278        Self: Sized,
279    {
280        let _ = tx_channels;
281        let _ = rx_channels;
282        Ok(Self::from_serial(S::try_new(config)?))
283    }
284
285    fn preprocess(&mut self, _clock: &RobotClock) -> CuResult<()> {
286        self.poll_serial()
287    }
288
289    fn send<'a, Payload>(
290        &mut self,
291        _clock: &RobotClock,
292        channel: &'static BridgeChannel<<Self::Tx as BridgeChannelSet>::Id, Payload>,
293        msg: &CuMsg<Payload>,
294    ) -> CuResult<()>
295    where
296        Payload: CuMsgPayload + 'a,
297    {
298        match channel.id() {
299            TxId::Requests => {
300                let request_msg: &CuMsg<MspRequestBatch> = msg.downcast_ref()?;
301                if let Some(batch) = request_msg.payload() {
302                    for request in batch.iter() {
303                        self.send_request(request)?;
304                    }
305                }
306            }
307        }
308        Ok(())
309    }
310
311    fn receive<'a, Payload>(
312        &mut self,
313        _clock: &RobotClock,
314        channel: &'static BridgeChannel<<Self::Rx as BridgeChannelSet>::Id, Payload>,
315        msg: &mut CuMsg<Payload>,
316    ) -> CuResult<()>
317    where
318        Payload: CuMsgPayload + 'a,
319    {
320        match channel.id() {
321            RxId::Responses => {
322                let response_msg: &mut CuMsg<MspResponseBatch> = msg.downcast_mut()?;
323                let mut batch = MspResponseBatch::new();
324                mem::swap(&mut batch, &mut self.pending_responses);
325                response_msg.set_payload(batch);
326            }
327        }
328        Ok(())
329    }
330}
331
332#[cfg(feature = "std")]
333pub mod std_serial {
334    use embedded_io_adapters::std::FromStd;
335    #[cfg(feature = "std")]
336    use std::boxed::Box;
337    #[cfg(feature = "std")]
338    use std::time::Duration;
339
340    pub type StdSerial = FromStd<Box<dyn serialport::SerialPort>>;
341
342    pub fn open(path: &str, baud: u32, timeout_ms: u64) -> std::io::Result<StdSerial> {
343        let port = serialport::new(path, baud)
344            .timeout(Duration::from_millis(timeout_ms))
345            .open()?;
346        Ok(FromStd::new(port))
347    }
348}
349
350/// Type alias for MSP bridge using standard I/O (for backward compatibility)
351#[cfg(feature = "std")]
352pub type CuMspBridgeStd = CuMspBridge<std_serial::StdSerial, std::io::Error>;
cu_msp_bridge/lib.rs

cu_msp_bridge/
lib.rs
