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            // A fixed-size type must match exactly; a variable-length one takes
136            // the indicated length as its (short) content.
137            if let Some(fixed) = data_type.fixed_size() {
138                if len != fixed {
139                    return abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh);
140                }
141            }
142            let value = match Value::decode_le(data_type, &req[4..4 + len]) {
143                Ok(v) => v,
144                Err(_) => return abort(addr, SdoAbortCode::General),
145            };
146            write_value(od, addr, value, &mut self.last_write)
147        } else {
148            // Segmented write: declare the size now, receive segments later.
149            let (_, size) = match decode_download_initiate_segmented(req) {
150                Ok(x) => x,
151                Err(_) => return abort(addr, SdoAbortCode::CommandInvalid),
152            };
153            // A fixed-size type must match; a variable-length one must fit the
154            // buffer.
155            let ok = match data_type.fixed_size() {
156                Some(fixed) => size as usize == fixed,
157                None => size as usize <= MAX_STRING_LEN,
158            };
159            if !ok {
160                return abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh);
161            }
162            self.transfer = Some(Transfer::Download {
163                addr,
164                data_type,
165                buf: Vec::new(),
166                declared: size as usize,
167                toggle: false,
168            });
169            Some(encode_download_response(addr))
170        }
171    }
172
173    fn on_upload_initiate<const N: usize>(
174        &mut self,
175        od: &mut ObjectDictionary<N>,
176        req: &SdoPayload,
177    ) -> Option<SdoPayload> {
178        let addr = req_address(req);
179        let value = match od.read(addr) {
180            Ok(v) => v,
181            Err(Error::ObjectNotFound) => return abort(addr, SdoAbortCode::ObjectDoesNotExist),
182            Err(Error::WriteOnly) => return abort(addr, SdoAbortCode::ReadOfWriteOnly),
183            Err(_) => return abort(addr, SdoAbortCode::General),
184        };
185        let size = value.size();
186        if size <= 4 {
187            Some(encode_upload_expedited_response(addr, &value).expect("size <= 4"))
188        } else {
189            let mut data = [0u8; MAX_STRING_LEN];
190            value
191                .encode_le(&mut data[..size])
192                .expect("size <= MAX_STRING_LEN");
193            self.transfer = Some(Transfer::Upload {
194                addr,
195                data,
196                len: size,
197                pos: 0,
198                toggle: false,
199            });
200            Some(encode_upload_initiate_segmented_response(addr, size as u32))
201        }
202    }
203
204    fn on_upload_segment(&mut self, req: &SdoPayload) -> Option<SdoPayload> {
205        let want_toggle = req[0] & TOGGLE != 0;
206        match self.transfer.take() {
207            Some(Transfer::Upload {
208                addr,
209                data,
210                len,
211                pos,
212                toggle,
213            }) => {
214                if want_toggle != toggle {
215                    return abort(addr, SdoAbortCode::ToggleBitNotAlternated);
216                }
217                let remaining = len - pos;
218                let n = remaining.min(SEGMENT_DATA_MAX);
219                let last = remaining <= SEGMENT_DATA_MAX;
220                let seg = encode_data_segment(&data[pos..pos + n], toggle, last).expect("1..=7");
221                if !last {
222                    self.transfer = Some(Transfer::Upload {
223                        addr,
224                        data,
225                        len,
226                        pos: pos + n,
227                        toggle: !toggle,
228                    });
229                }
230                Some(seg)
231            }
232            _ => abort(Address::new(0, 0), SdoAbortCode::CommandInvalid),
233        }
234    }
235
236    fn on_download_segment<const N: usize>(
237        &mut self,
238        od: &mut ObjectDictionary<N>,
239        req: &SdoPayload,
240    ) -> Option<SdoPayload> {
241        let seg = match decode_data_segment(req) {
242            Ok(s) => s,
243            Err(_) => return abort(Address::new(0, 0), SdoAbortCode::CommandInvalid),
244        };
245        match self.transfer.take() {
246            Some(Transfer::Download {
247                addr,
248                data_type,
249                mut buf,
250                declared,
251                toggle,
252            }) => {
253                if seg.toggle != toggle {
254                    return abort(addr, SdoAbortCode::ToggleBitNotAlternated);
255                }
256                if buf.extend_from_slice(seg.data).is_err() {
257                    return abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh);
258                }
259                let ack = encode_download_segment_response(seg.toggle);
260                if seg.last {
261                    if buf.len() != declared {
262                        return abort(addr, SdoAbortCode::DataTypeMismatchLengthLow);
263                    }
264                    let value = match Value::decode_le(data_type, &buf) {
265                        Ok(v) => v,
266                        Err(_) => return abort(addr, SdoAbortCode::General),
267                    };
268                    // The final segment is acknowledged with a segment
269                    // response, not a download-initiate response.
270                    match od.write(addr, value) {
271                        Ok(()) => {
272                            self.last_write = Some(addr);
273                            Some(ack)
274                        }
275                        Err(Error::ReadOnly) => abort(addr, SdoAbortCode::WriteOfReadOnly),
276                        Err(_) => abort(addr, SdoAbortCode::General),
277                    }
278                } else {
279                    self.transfer = Some(Transfer::Download {
280                        addr,
281                        data_type,
282                        buf,
283                        declared,
284                        toggle: !toggle,
285                    });
286                    Some(ack)
287                }
288            }
289            _ => abort(Address::new(0, 0), SdoAbortCode::CommandInvalid),
290        }
291    }
292}
293
294/// Write `value` to `od`, recording the address on success and mapping OD
295/// errors to SDO abort responses.
296fn write_value<const N: usize>(
297    od: &mut ObjectDictionary<N>,
298    addr: Address,
299    value: Value,
300    last_write: &mut Option<Address>,
301) -> Option<SdoPayload> {
302    match od.write(addr, value) {
303        Ok(()) => {
304            *last_write = Some(addr);
305            Some(encode_download_response(addr))
306        }
307        Err(Error::ReadOnly) => abort(addr, SdoAbortCode::WriteOfReadOnly),
308        Err(Error::TypeMismatch) => abort(addr, SdoAbortCode::DataTypeMismatchLengthHigh),
309        Err(_) => abort(addr, SdoAbortCode::General),
310    }
311}
312
313fn req_address(req: &SdoPayload) -> Address {
314    Address::new(u16::from_le_bytes([req[1], req[2]]), req[3])
315}
316
317fn abort(addr: Address, code: SdoAbortCode) -> Option<SdoPayload> {
318    Some(encode_abort(addr, code))
319}
320
321#[cfg(test)]
322mod tests {
323    use super::*;
324    use crate::object_dictionary::Entry;
325
326    fn server() -> SdoServer {
327        SdoServer::new(NodeId::new(1).unwrap())
328    }
329
330    fn od() -> ObjectDictionary<8> {
331        let mut od = ObjectDictionary::new();
332        od.insert(
333            Address::new(0x1000, 0),
334            Entry::constant(Value::Unsigned32(0x0000_0192)),
335        )
336        .unwrap();
337        od.insert(Address::new(0x1017, 0), Entry::rw(Value::Unsigned16(1000)))
338            .unwrap();
339        od.insert(Address::new(0x2000, 0), Entry::rw(Value::Unsigned64(0)))
340            .unwrap();
341        od
342    }
343
344    #[test]
345    fn cob_ids_track_node() {
346        let s = SdoServer::new(NodeId::new(5).unwrap());
347        assert_eq!(s.request_cob_id(), 0x605);
348        assert_eq!(s.response_cob_id(), 0x585);
349    }
350
351    #[test]
352    fn expedited_read_returns_value() {
353        let mut od = od();
354        let req = super::super::encode_upload_request(Address::new(0x1000, 0));
355        let resp = server().handle(&mut od, &req).unwrap();
356        let (_, value) =
357            super::super::decode_upload_expedited_response(&resp, DataType::Unsigned32).unwrap();
358        assert_eq!(value, Value::Unsigned32(0x192));
359    }
360
361    #[test]
362    fn expedited_write_updates_od() {
363        let mut od = od();
364        let req = super::super::encode_download_expedited(
365            Address::new(0x1017, 0),
366            &Value::Unsigned16(1234),
367        )
368        .unwrap();
369        let resp = server().handle(&mut od, &req).unwrap();
370        assert!(super::super::decode_download_response(&resp).is_ok());
371        assert_eq!(
372            od.read(Address::new(0x1017, 0)).unwrap(),
373            Value::Unsigned16(1234)
374        );
375    }
376
377    #[test]
378    fn take_write_reports_the_written_object_once() {
379        let mut od = od();
380        let mut s = server();
381        // Nothing written yet.
382        assert_eq!(s.take_write(), None);
383
384        let req = super::super::encode_download_expedited(
385            Address::new(0x1017, 0),
386            &Value::Unsigned16(1234),
387        )
388        .unwrap();
389        s.handle(&mut od, &req).unwrap();
390        // The write is reported once, then cleared.
391        assert_eq!(s.take_write(), Some(Address::new(0x1017, 0)));
392        assert_eq!(s.take_write(), None);
393
394        // A failed write (read-only object) is not reported.
395        let ro =
396            super::super::encode_download_expedited(Address::new(0x1000, 0), &Value::Unsigned32(1))
397                .unwrap();
398        s.handle(&mut od, &ro).unwrap();
399        assert_eq!(s.take_write(), None);
400    }
401
402    #[test]
403    fn read_missing_object_aborts() {
404        let mut od = od();
405        let req = super::super::encode_upload_request(Address::new(0x9999, 0));
406        let resp = server().handle(&mut od, &req).unwrap();
407        let (_, code) = super::super::decode_abort(&resp).unwrap();
408        assert_eq!(code, 0x0602_0000);
409    }
410
411    #[test]
412    fn write_read_only_aborts() {
413        let mut od = od();
414        let req =
415            super::super::encode_download_expedited(Address::new(0x1000, 0), &Value::Unsigned32(1))
416                .unwrap();
417        let resp = server().handle(&mut od, &req).unwrap();
418        let (_, code) = super::super::decode_abort(&resp).unwrap();
419        assert_eq!(code, 0x0601_0002); // write of a read-only object
420    }
421
422    #[test]
423    fn client_abort_clears_state_without_reply() {
424        let mut od = od();
425        let mut s = server();
426        // Start a segmented download so the server is busy.
427        let init = super::super::encode_download_initiate_segmented(Address::new(0x2000, 0), 8);
428        s.handle(&mut od, &init).unwrap();
429        assert!(s.is_busy());
430        let abort_frame =
431            super::super::encode_abort(Address::new(0x2000, 0), SdoAbortCode::General);
432        assert!(s.handle(&mut od, &abort_frame).is_none());
433        assert!(!s.is_busy());
434    }
435}
canopen_rs/sdo/server.rs

canopen_rs/sdo/
server.rs
