canopen_rs/sdo/
server.rs

1//! SDO server — services requests against an object dictionary.
2//!
3//! [`SdoServer`] is a sans-I/O state machine: feed it a decoded request frame
4//! with [`SdoServer::handle`] and it reads or writes the supplied
5//! [`ObjectDictionary`], returning the response frame to transmit (or `None`
6//! for a client abort, which needs no reply). It handles expedited transfers
7//! in a single exchange and drives segmented transfers — needed for values of
8//! five to eight bytes, such as `UNSIGNED64` — across multiple frames, holding
9//! the per-transfer state between calls.
10
11use heapless::Vec;
12
13use super::{
14    decode_data_segment, decode_download_initiate_segmented, encode_abort, encode_data_segment,
15    encode_download_response, encode_download_segment_response, encode_upload_expedited_response,
16    encode_upload_initiate_segmented_response, request_cob_id, response_cob_id, SdoAbortCode,
17    SdoPayload, CCS_DOWNLOAD_INITIATE, CCS_DOWNLOAD_SEGMENT, CCS_UPLOAD_INITIATE,
18    CCS_UPLOAD_SEGMENT, CS_ABORT, CS_MASK, EXPEDITED, SEGMENT_DATA_MAX, SIZE_INDICATED, TOGGLE,
19};
20use crate::datatypes::{DataType, Value, MAX_STRING_LEN};
21use crate::object_dictionary::{Address, ObjectDictionary};
22use crate::types::NodeId;
23use crate::Error;
24
25/// An in-progress segmented transfer held between frames.
26///
27/// The buffers are sized to [`MAX_STRING_LEN`], the largest value the server
28/// transfers — eight bytes covers every numeric type, and the rest is headroom
29/// for the variable-length (`VISIBLE_STRING` / `OCTET_STRING` / `DOMAIN`) types.
30#[derive(Debug)]
31enum Transfer {
32    /// The client is reading a value larger than four bytes from us.
33    Upload {
34        addr: Address,
35        data: [u8; MAX_STRING_LEN],
36        len: usize,
37        pos: usize,
38        toggle: bool,
39    },
40    /// The client is writing a value larger than four bytes to us.
41    Download {
42        addr: Address,
43        data_type: DataType,
44        buf: Vec<u8, MAX_STRING_LEN>,
45        declared: usize,
46        toggle: bool,
47    },
48}
49
50/// An SDO server bound to a node id, servicing requests against an object
51/// dictionary.
52#[derive(Debug)]
53pub struct SdoServer {
54    node: NodeId,
55    transfer: Option<Transfer>,
56    last_write: Option<Address>,
57}
58
59impl SdoServer {
60    /// Create a server for `node`.
61    pub const fn new(node: NodeId) -> Self {
62        Self {
63            node,
64            transfer: None,
65            last_write: None,
66        }
67    }
68
69    /// The COB-ID this server receives requests on (`0x600 + node`).
70    pub fn request_cob_id(&self) -> u16 {
71        request_cob_id(self.node)
72    }
73
74    /// The COB-ID this server sends responses on (`0x580 + node`).
75    pub fn response_cob_id(&self) -> u16 {
76        response_cob_id(self.node)
77    }
78
79    /// Whether a segmented transfer is currently in progress.
80    pub fn is_busy(&self) -> bool {
81        self.transfer.is_some()
82    }
83
84    /// Take the address of the object most recently written by a completed SDO
85    /// download, clearing it. Poll after [`SdoServer::handle`] to react to a
86    /// master's writes (e.g. re-read configuration). `None` if nothing was
87    /// written since the last call.
88    pub fn take_write(&mut self) -> Option<Address> {
89        self.last_write.take()
90    }
91
92    /// Handle an SDO request against `od`, returning the response to transmit.
93    ///
94    /// Returns `None` only for a client abort, which is unconfirmed. Any
95    /// protocol or access error produces an SDO abort response frame.
96    pub fn handle<const N: usize>(
97        &mut self,
98        od: &mut ObjectDictionary<N>,
99        req: &SdoPayload,
100    ) -> Option<SdoPayload> {
101        match req[0] & CS_MASK {
102            CCS_DOWNLOAD_INITIATE => self.on_download_initiate(od, req),
103            CCS_UPLOAD_INITIATE => self.on_upload_initiate(od, req),
104            CCS_DOWNLOAD_SEGMENT => self.on_download_segment(od, req),
105            CCS_UPLOAD_SEGMENT => self.on_upload_segment(req),
106            CS_ABORT => {
107                self.transfer = None;
108                None
109            }
110            _ => abort(req_address(req), SdoAbortCode::CommandInvalid),
111        }
112    }
113
114    fn on_download_initiate<const N: usize>(
115        &mut self,
116        od: &mut ObjectDictionary<N>,
117        req: &SdoPayload,
118    ) -> Option<SdoPayload> {
119        let addr = req_address(req);
120        let (data_type, writable) = match od.entry(addr) {
121            Some(e) => (e.value.data_type(), e.access.is_writable()),
122            None => return abort(addr, SdoAbortCode::ObjectDoesNotExist),
123        };
124        if !writable {
125            return abort(addr, SdoAbortCode::WriteOfReadOnly);
126        }
127
128        if req[0] & EXPEDITED != 0 {
129            // Expedited write: 0..=4 data bytes inline.
130            let len = if req[0] & SIZE_INDICATED != 0 {
131                4 - ((req[0] >> 2) & 0x03) as usize
132            } else {
133                data_type.fixed_size().unwrap_or(0)
134            };
135            // Expedited carries at most four data bytes; a larger fixed type
136            // must use segmented transfer. Guard the slice against a malformed
137            // frame (e.g. size-indication clear on an 8-byte object).
138            if len > 4 {
139                return abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh);
140            }
141            // A fixed-size type must match exactly; a variable-length one takes
142            // the indicated length as its (short) content.
143            if let Some(fixed) = data_type.fixed_size() {
144                if len != fixed {
145                    return abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh);
146                }
147            }
148            let value = match Value::decode_le(data_type, &req[4..4 + len]) {
149                Ok(v) => v,
150                Err(_) => return abort(addr, SdoAbortCode::General),
151            };
152            write_value(od, addr, value, &mut self.last_write)
153        } else {
154            // Segmented write: declare the size now, receive segments later.
155            let (_, size) = match decode_download_initiate_segmented(req) {
156                Ok(x) => x,
157                Err(_) => return abort(addr, SdoAbortCode::CommandInvalid),
158            };
159            // A fixed-size type must match; a variable-length one must fit the
160            // buffer.
161            let ok = match data_type.fixed_size() {
162                Some(fixed) => size as usize == fixed,
163                None => size as usize <= MAX_STRING_LEN,
164            };
165            if !ok {
166                return abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh);
167            }
168            self.transfer = Some(Transfer::Download {
169                addr,
170                data_type,
171                buf: Vec::new(),
172                declared: size as usize,
173                toggle: false,
174            });
175            Some(encode_download_response(addr))
176        }
177    }
178
179    fn on_upload_initiate<const N: usize>(
180        &mut self,
181        od: &mut ObjectDictionary<N>,
182        req: &SdoPayload,
183    ) -> Option<SdoPayload> {
184        let addr = req_address(req);
185        let value = match od.read(addr) {
186            Ok(v) => v,
187            Err(Error::ObjectNotFound) => return abort(addr, SdoAbortCode::ObjectDoesNotExist),
188            Err(Error::WriteOnly) => return abort(addr, SdoAbortCode::ReadOfWriteOnly),
189            Err(_) => return abort(addr, SdoAbortCode::General),
190        };
191        let size = value.size();
192        if (1..=4).contains(&size) {
193            Some(encode_upload_expedited_response(addr, &value).expect("size 1..=4"))
194        } else {
195            let mut data = [0u8; MAX_STRING_LEN];
196            value
197                .encode_le(&mut data[..size])
198                .expect("size <= MAX_STRING_LEN");
199            self.transfer = Some(Transfer::Upload {
200                addr,
201                data,
202                len: size,
203                pos: 0,
204                toggle: false,
205            });
206            Some(encode_upload_initiate_segmented_response(addr, size as u32))
207        }
208    }
209
210    fn on_upload_segment(&mut self, req: &SdoPayload) -> Option<SdoPayload> {
211        let want_toggle = req[0] & TOGGLE != 0;
212        match self.transfer.take() {
213            Some(Transfer::Upload {
214                addr,
215                data,
216                len,
217                pos,
218                toggle,
219            }) => {
220                if want_toggle != toggle {
221                    return abort(addr, SdoAbortCode::ToggleBitNotAlternated);
222                }
223                let remaining = len - pos;
224                let n = remaining.min(SEGMENT_DATA_MAX);
225                let last = remaining <= SEGMENT_DATA_MAX;
226                let seg = encode_data_segment(&data[pos..pos + n], toggle, last).expect("1..=7");
227                if !last {
228                    self.transfer = Some(Transfer::Upload {
229                        addr,
230                        data,
231                        len,
232                        pos: pos + n,
233                        toggle: !toggle,
234                    });
235                }
236                Some(seg)
237            }
238            _ => abort(Address::new(0, 0), SdoAbortCode::CommandInvalid),
239        }
240    }
241
242    fn on_download_segment<const N: usize>(
243        &mut self,
244        od: &mut ObjectDictionary<N>,
245        req: &SdoPayload,
246    ) -> Option<SdoPayload> {
247        let seg = match decode_data_segment(req) {
248            Ok(s) => s,
249            Err(_) => return abort(Address::new(0, 0), SdoAbortCode::CommandInvalid),
250        };
251        match self.transfer.take() {
252            Some(Transfer::Download {
253                addr,
254                data_type,
255                mut buf,
256                declared,
257                toggle,
258            }) => {
259                if seg.toggle != toggle {
260                    return abort(addr, SdoAbortCode::ToggleBitNotAlternated);
261                }
262                if buf.extend_from_slice(seg.data).is_err() {
263                    return abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh);
264                }
265                let ack = encode_download_segment_response(seg.toggle);
266                if seg.last {
267                    if buf.len() != declared {
268                        return abort(addr, SdoAbortCode::DataTypeMismatchLengthLow);
269                    }
270                    let value = match Value::decode_le(data_type, &buf) {
271                        Ok(v) => v,
272                        Err(_) => return abort(addr, SdoAbortCode::General),
273                    };
274                    // The final segment is acknowledged with a segment
275                    // response, not a download-initiate response.
276                    match od.write(addr, value) {
277                        Ok(()) => {
278                            self.last_write = Some(addr);
279                            Some(ack)
280                        }
281                        Err(Error::ReadOnly) => abort(addr, SdoAbortCode::WriteOfReadOnly),
282                        Err(_) => abort(addr, SdoAbortCode::General),
283                    }
284                } else {
285                    self.transfer = Some(Transfer::Download {
286                        addr,
287                        data_type,
288                        buf,
289                        declared,
290                        toggle: !toggle,
291                    });
292                    Some(ack)
293                }
294            }
295            _ => abort(Address::new(0, 0), SdoAbortCode::CommandInvalid),
296        }
297    }
298}
299
300/// Write `value` to `od`, recording the address on success and mapping OD
301/// errors to SDO abort responses.
302fn write_value<const N: usize>(
303    od: &mut ObjectDictionary<N>,
304    addr: Address,
305    value: Value,
306    last_write: &mut Option<Address>,
307) -> Option<SdoPayload> {
308    match od.write(addr, value) {
309        Ok(()) => {
310            *last_write = Some(addr);
311            Some(encode_download_response(addr))
312        }
313        Err(Error::ReadOnly) => abort(addr, SdoAbortCode::WriteOfReadOnly),
314        Err(Error::TypeMismatch) => abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh),
315        Err(_) => abort(addr, SdoAbortCode::General),
316    }
317}
318
319fn req_address(req: &SdoPayload) -> Address {
320    Address::new(u16::from_le_bytes([req[1], req[2]]), req[3])
321}
322
323fn abort(addr: Address, code: SdoAbortCode) -> Option<SdoPayload> {
324    Some(encode_abort(addr, code))
325}
326
327#[cfg(test)]
328mod tests {
329    use super::*;
330    use crate::object_dictionary::Entry;
331
332    fn server() -> SdoServer {
333        SdoServer::new(NodeId::new(1).unwrap())
334    }
335
336    fn od() -> ObjectDictionary<8> {
337        let mut od = ObjectDictionary::new();
338        od.insert(
339            Address::new(0x1000, 0),
340            Entry::constant(Value::Unsigned32(0x0000_0192)),
341        )
342        .unwrap();
343        od.insert(Address::new(0x1017, 0), Entry::rw(Value::Unsigned16(1000)))
344            .unwrap();
345        od.insert(Address::new(0x2000, 0), Entry::rw(Value::Unsigned64(0)))
346            .unwrap();
347        od
348    }
349
350    #[test]
351    fn cob_ids_track_node() {
352        let s = SdoServer::new(NodeId::new(5).unwrap());
353        assert_eq!(s.request_cob_id(), 0x605);
354        assert_eq!(s.response_cob_id(), 0x585);
355    }
356
357    #[test]
358    fn expedited_read_returns_value() {
359        let mut od = od();
360        let req = super::super::encode_upload_request(Address::new(0x1000, 0));
361        let resp = server().handle(&mut od, &req).unwrap();
362        let (_, value) =
363            super::super::decode_upload_expedited_response(&resp, DataType::Unsigned32).unwrap();
364        assert_eq!(value, Value::Unsigned32(0x192));
365    }
366
367    #[test]
368    fn expedited_write_updates_od() {
369        let mut od = od();
370        let req = super::super::encode_download_expedited(
371            Address::new(0x1017, 0),
372            &Value::Unsigned16(1234),
373        )
374        .unwrap();
375        let resp = server().handle(&mut od, &req).unwrap();
376        assert!(super::super::decode_download_response(&resp).is_ok());
377        assert_eq!(
378            od.read(Address::new(0x1017, 0)).unwrap(),
379            Value::Unsigned16(1234)
380        );
381    }
382
383    #[test]
384    fn take_write_reports_the_written_object_once() {
385        let mut od = od();
386        let mut s = server();
387        // Nothing written yet.
388        assert_eq!(s.take_write(), None);
389
390        let req = super::super::encode_download_expedited(
391            Address::new(0x1017, 0),
392            &Value::Unsigned16(1234),
393        )
394        .unwrap();
395        s.handle(&mut od, &req).unwrap();
396        // The write is reported once, then cleared.
397        assert_eq!(s.take_write(), Some(Address::new(0x1017, 0)));
398        assert_eq!(s.take_write(), None);
399
400        // A failed write (read-only object) is not reported.
401        let ro =
402            super::super::encode_download_expedited(Address::new(0x1000, 0), &Value::Unsigned32(1))
403                .unwrap();
404        s.handle(&mut od, &ro).unwrap();
405        assert_eq!(s.take_write(), None);
406    }
407
408    #[test]
409    fn read_missing_object_aborts() {
410        let mut od = od();
411        let req = super::super::encode_upload_request(Address::new(0x9999, 0));
412        let resp = server().handle(&mut od, &req).unwrap();
413        let (_, code) = super::super::decode_abort(&resp).unwrap();
414        assert_eq!(code, 0x0602_0000);
415    }
416
417    #[test]
418    fn write_read_only_aborts() {
419        let mut od = od();
420        let req =
421            super::super::encode_download_expedited(Address::new(0x1000, 0), &Value::Unsigned32(1))
422                .unwrap();
423        let resp = server().handle(&mut od, &req).unwrap();
424        let (_, code) = super::super::decode_abort(&resp).unwrap();
425        assert_eq!(code, 0x0601_0002); // write of a read-only object
426    }
427
428    #[test]
429    fn client_abort_clears_state_without_reply() {
430        let mut od = od();
431        let mut s = server();
432        // Start a segmented download so the server is busy.
433        let init = super::super::encode_download_initiate_segmented(Address::new(0x2000, 0), 8);
434        s.handle(&mut od, &init).unwrap();
435        assert!(s.is_busy());
436        let abort_frame =
437            super::super::encode_abort(Address::new(0x2000, 0), SdoAbortCode::General);
438        assert!(s.handle(&mut od, &abort_frame).is_none());
439        assert!(!s.is_busy());
440    }
441}
canopen_rs/sdo/server.rs

canopen_rs/sdo/
server.rs
