canopeners/
lib.rs

1//! # Canopeners
2//! Partial implementation of a CANOpen client 🔪🥫 on top of socketcan
3//! Examples in readme
4//!
5//! # CANOpen
6//! As a [layer 1 and 2](https://en.wikipedia.org/wiki/OSI_model) protocol, CAN does not support
7//! addressing - all messages arrive at all nodes. CANOpen mainly adds addressing (layer 3) support
8//! to CAN in a standardized way. All CANOpen nodes have an ID, there's a standard way to address
9//! a message to a node.
10//! Various CANOpen specs also include tons of other features, this repo just focuses on CiA301.
11//!
12//! # Progress
13//! So far, we have:
14//! ✅ rusty types for most CANOpen messages
15//! ✅ send/receive messages via socketcan
16//! ✅ nice SDO wrapper.
17//! we're still missing:
18//! ❌CANOpen node (read/writable Object Dictionary, respecting the OD configs)
19//! ❌MPDO support
20//!
21
22use std::num::TryFromIntError;
23
24use binrw::{binrw, BinRead, BinWrite};
25use socketcan::{EmbeddedFrame, Frame, Id, Socket};
26
27pub mod enums;
28
29trait FrameRW {
30    fn encode(&self, frame: &mut socketcan::CanFrame);
31    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError>
32    where
33        Self: Sized;
34}
35
36/// CAN ids can only be standard (11bit) or extended (29bit)
37/// CANOpen only uses the standard id bits
38/// when using extended ids, the extra bits are 0
39/// for now, this library only uses standard ids
40fn id_as_raw_std(frame: &socketcan::CanFrame) -> Result<u16, CanOpenError> {
41    if let Id::Standard(sid) = frame.id() {
42        Ok(sid.as_raw())
43    } else {
44        Err(CanOpenError::CanVersion(
45            "got extended (29bit) id, expected standard (11bit) id".to_owned(),
46        ))
47    }
48}
49
50// todo: I think node_ids are u8s actually
51fn u16_as_id(id: u16) -> socketcan::StandardId {
52    socketcan::StandardId::new(id).unwrap()
53}
54
55#[binrw]
56#[brw(little)]
57#[derive(Clone, Debug)]
58pub struct Nmt {
59    pub function: NmtFunction,
60    pub target_node: u8,
61}
62
63impl Nmt {
64    pub fn new(function: NmtFunction, target_node: u8) -> Self {
65        Self {
66            function,
67            target_node,
68        }
69    }
70}
71
72impl FrameRW for Nmt {
73    fn decode(frame: &socketcan::CanFrame) -> Result<Nmt, CanOpenError> {
74        let mut c = std::io::Cursor::new(frame.data());
75        Nmt::read(&mut c).map_err(|binrw_err| CanOpenError::ParseError(binrw_err.to_string()))
76    }
77
78    fn encode(&self, frame: &mut socketcan::CanFrame) {
79        frame.set_id(u16_as_id(0x000));
80        let mut c = std::io::Cursor::new(Vec::new());
81        self.write(&mut c).unwrap();
82        frame.set_data(c.get_ref()).unwrap();
83    }
84}
85
86#[binrw]
87#[br(repr(u8))]
88#[bw(repr(u8))]
89#[derive(Clone, Debug)]
90pub enum NmtFunction {
91    StartRemoteNode = 0x01,
92    StopRemoteNode = 0x02,
93    EnterPreOperational = 0x80,
94    ResetNode = 0x81,
95    ResetCommunication = 0x82,
96}
97
98#[binrw]
99#[brw(little)]
100#[derive(Debug)]
101pub struct Emergency {
102    #[brw(ignore)]
103    node_id: u8,
104
105    #[br(temp)]
106    #[bw(calc =  enums::EmergencyErrorCode::encode(error_code))]
107    error_code_raw: u16,
108
109    #[br(try_calc = enums::EmergencyErrorCode::decode(error_code_raw))]
110    #[bw(ignore)]
111    error_code: enums::EmergencyErrorCode,
112
113    #[br(temp)]
114    #[bw(calc = enums::EmergencyErrorRegister::encode(error_register))]
115    error_register_raw: u8,
116
117    #[br(calc = enums::EmergencyErrorRegister::decode(error_register_raw))]
118    #[bw(ignore)]
119    error_register: Vec<enums::EmergencyErrorRegister>,
120
121    vendor_specific: [u8; 5],
122}
123
124impl Emergency {
125    pub fn new(
126        node_id: u8,
127        error_code: enums::EmergencyErrorCode,
128        error_register: Vec<enums::EmergencyErrorRegister>,
129        vendor_specific: &[u8],
130    ) -> Self {
131        Self {
132            node_id,
133            error_code,
134            error_register,
135            vendor_specific: Self::to_vendor_specific(vendor_specific),
136        }
137    }
138
139    fn to_vendor_specific(data: &[u8]) -> [u8; 5] {
140        let mut arr = [0u8; 5];
141        arr[0..data.len()].copy_from_slice(data);
142        arr
143    }
144}
145
146impl FrameRW for Emergency {
147    fn decode(frame: &socketcan::CanFrame) -> Result<Emergency, CanOpenError> {
148        let data = frame.data();
149        if data.len() < 8 {
150            return Err(CanOpenError::ParseError(
151                "not a valid Emergency message, need at least 8 bytes".to_owned(),
152            ));
153        }
154        let id = id_as_raw_std(frame)?;
155        Emergency::read(&mut std::io::Cursor::new(data))
156            .map_err(|e| CanOpenError::ParseError(format!("binrw err: {e}")))
157            .map(|mut m| {
158                m.node_id = (id - 0x080) as u8;
159                m
160            })
161    }
162
163    fn encode(&self, frame: &mut socketcan::CanFrame) {
164        frame.set_id(u16_as_id(0x80 + self.node_id as u16));
165        let mut c = std::io::Cursor::new(Vec::new());
166        self.write(&mut c).unwrap();
167        frame.set_data(c.get_ref()).unwrap();
168    }
169}
170
171#[derive(Clone, Debug)]
172pub struct Sdo {
173    pub node_id: u8,     // Derived from the header
174    pub command: SdoCmd, // command specifier
175    pub reqres: ReqRes,
176}
177
178#[derive(Clone, Debug)]
179pub enum SdoCmd {
180    DownloadSegmentTx(SdoCmdDownloadSegmentTx),
181    InitiateDownloadTx(SdoCmdInitiateDownloadTx),
182    InitiateUploadTx(SdoCmdInitiateUploadTx),
183    UploadSegmentTx(SdoCmdUploadSegmentTx),
184    BlockUploadTx,
185    BlockDownloadTx,
186
187    DownloadSegmentRx(SdoCmdDownloadSegmentRx),
188    InitiateDownloadRx(SdoCmdInitiateDownloadRx),
189    InitiateUploadRx(SdoCmdInitiateUploadRx),
190    UploadSegmentRx(SdoCmdUploadSegmentRx),
191    BlockUploadRx,
192    BlockDownloadRx,
193
194    AbortTransfer(SdoCmdAbortTransfer),
195}
196
197impl SdoCmd {
198    #[allow(clippy::match_like_matches_macro)]
199    pub fn is_response_to(a: &Self, b: &Self) -> bool {
200        use SdoCmd::*;
201        match (a, b) {
202            (DownloadSegmentRx(_), DownloadSegmentTx(_)) => true,
203            (InitiateDownloadRx(_), InitiateDownloadTx(_)) => true,
204            (InitiateUploadRx(_), InitiateUploadTx(_)) => true,
205            (UploadSegmentRx(_), UploadSegmentTx(_)) => true,
206            (BlockUploadRx, BlockUploadTx) => true,
207            (BlockDownloadRx, BlockDownloadTx) => true,
208            _ => false,
209        }
210    }
211}
212
213#[derive(Clone, Debug)]
214pub struct SdoCmdInitiateDownloadRx {
215    pub index: u16,
216    pub sub_index: u8,
217    // reused for InitiateUploadTx
218    pub payload: SdoCmdInitiatePayload,
219}
220
221#[derive(Clone, Debug)]
222pub enum SdoCmdInitiatePayload {
223    Expedited(Box<[u8]>), // in expedited sdo, InitiateDownload carries up to 4 payload bytes
224    Segmented(Option<u32>), // in segmented sdo, InitiateDownload may indicate size of data to be
225                          // transmitted in subsequent segments
226}
227
228impl SdoCmdInitiatePayload {
229    fn encode(&self, frame: &mut socketcan::CanFrame) {
230        let mut data = [0u8; 8];
231        data.copy_from_slice(frame.data());
232        let mut command_byte = data[0];
233        match self {
234            SdoCmdInitiatePayload::Expedited(exp_data) => {
235                let l = match exp_data.len() {
236                    1 => 0b11,
237                    2 => 0b10,
238                    3 => 0b01,
239                    4 => 0b00,
240                    _ => unreachable!(),
241                } << 2;
242                command_byte |= l;
243                command_byte |= 0b11;
244                data[4..4 + exp_data.len()].copy_from_slice(exp_data);
245            }
246            SdoCmdInitiatePayload::Segmented(Some(size)) => {
247                command_byte |= 0b01;
248                data[4..8].copy_from_slice(&size.to_le_bytes());
249            }
250            SdoCmdInitiatePayload::Segmented(None) => command_byte |= 0b00,
251        };
252        data[0] = command_byte;
253
254        frame.set_data(&data).unwrap();
255    }
256
257    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
258        let size_indicated = frame.data()[0] & 0b1 != 0;
259        let expedited = frame.data()[0] & 0b10 != 0;
260        if expedited {
261            // "size indicated" bit
262            let l = if size_indicated {
263                match (frame.data()[0] & 0b1100) >> 2 {
264                    0b11 => 1,
265                    0b10 => 2,
266                    0b01 => 3,
267                    0b00 => 4,
268                    // this path is technically unreachable, it must be a regression
269                    _ => {
270                        return Err(CanOpenError::ParseError(
271                            "logic bug while decoding sdo".to_owned(),
272                        ))
273                    }
274                }
275            } else {
276                // data size not indicated, assume max
277                4
278            };
279
280            let mut data = Vec::with_capacity(l);
281            data.extend_from_slice(&frame.data()[4..4 + l]);
282            let payload = SdoCmdInitiatePayload::Expedited(data.into());
283            Ok(payload)
284        } else {
285            let size = if size_indicated {
286                let size = u32::from_le_bytes(frame.data()[4..8].try_into().unwrap());
287                Some(size)
288            } else {
289                None
290            };
291            Ok(SdoCmdInitiatePayload::Segmented(size))
292        }
293    }
294}
295
296impl SdoCmdInitiateDownloadRx {
297    fn encode(&self, frame: &mut socketcan::CanFrame) {
298        let mut data = [0u8; 8];
299        let command_byte = 0b00100000;
300
301        data[0] = command_byte;
302        data[1..3].copy_from_slice(&self.index.to_le_bytes());
303        data[3] = self.sub_index;
304        frame.set_data(&data).unwrap();
305
306        self.payload.encode(frame);
307    }
308
309    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
310        let index = u16::from_le_bytes(
311            frame.data()[1..3]
312                .try_into()
313                .map_err(|_| CanOpenError::ParseError("not enough data".to_owned()))?,
314        );
315        let sub_index = frame.data()[3];
316        let payload = SdoCmdInitiatePayload::decode(frame)?;
317        Ok(Self {
318            index,
319            sub_index,
320            payload,
321        })
322    }
323}
324
325#[derive(Clone, Debug)]
326pub struct SdoCmdInitiateDownloadTx {
327    pub index: u16,
328    pub sub_index: u8,
329}
330
331impl SdoCmdInitiateDownloadTx {
332    fn encode(&self, frame: &mut socketcan::CanFrame) {
333        let mut data = [0u8; 8];
334        data[0] = 0b01100000;
335        data[1..3].copy_from_slice(&self.index.to_le_bytes());
336        data[3] = self.sub_index;
337        frame.set_data(&data).unwrap();
338    }
339
340    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
341        let index = u16::from_le_bytes(
342            frame.data()[1..3]
343                .try_into()
344                .map_err(|_| CanOpenError::ParseError("not enough data".to_owned()))?,
345        );
346        let sub_index = frame.data()[3];
347
348        Ok(Self { index, sub_index })
349    }
350}
351
352#[derive(Clone, Debug)]
353pub struct SdoCmdDownloadSegmentRx {
354    pub toggle: bool,
355    pub data: Box<[u8]>,
356    pub last: bool,
357}
358
359impl SdoCmdDownloadSegmentRx {
360    fn encode(&self, frame: &mut socketcan::CanFrame) {
361        let mut data = [0u8; 8];
362        data[0] =
363            ((self.toggle as u8) << 4) | ((7 - self.data.len() as u8) << 1) | (self.last as u8);
364        data[1..1 + self.data.len()].copy_from_slice(&self.data);
365        frame.set_data(&data).unwrap();
366    }
367
368    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
369        let command_byte = frame.data()[0];
370        let toggle = command_byte & 0b10000 != 0;
371        let size = 7 - (0b111 & (command_byte >> 1)) as usize;
372        let last = command_byte & 0b1 != 0;
373        let mut data = Vec::new();
374        data.extend_from_slice(&frame.data()[1..1 + size]);
375
376        Ok(Self {
377            toggle,
378            last,
379            data: data.into(),
380        })
381    }
382}
383
384#[derive(Clone, Debug)]
385pub struct SdoCmdDownloadSegmentTx {
386    pub toggle: bool,
387}
388
389impl SdoCmdDownloadSegmentTx {
390    fn encode(&self, frame: &mut socketcan::CanFrame) {
391        let mut data = [0u8; 8];
392        data[0] = 0b001 << 5 | ((self.toggle as u8) << 4);
393        frame.set_data(&data).unwrap();
394    }
395
396    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
397        let command_byte = frame.data()[0];
398        let toggle = command_byte & 0b10000 != 0;
399        Ok(Self { toggle })
400    }
401}
402
403#[derive(Clone, Debug)]
404pub struct SdoCmdInitiateUploadRx {
405    pub index: u16,
406    pub sub_index: u8,
407}
408
409impl SdoCmdInitiateUploadRx {
410    fn encode(&self, frame: &mut socketcan::CanFrame) {
411        let mut data = [0u8; 8];
412        let command_byte = 0b010 << 5;
413        data[0] = command_byte;
414        data[1..3].copy_from_slice(&self.index.to_le_bytes());
415        data[3] = self.sub_index;
416        frame.set_data(&data).unwrap();
417    }
418
419    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
420        let index = u16::from_le_bytes(
421            frame.data()[1..3]
422                .try_into()
423                .map_err(|_| CanOpenError::ParseError("not enough data".to_owned()))?,
424        );
425        let sub_index = frame.data()[3];
426        Ok(Self { index, sub_index })
427    }
428}
429
430#[derive(Clone, Debug)]
431pub struct SdoCmdInitiateUploadTx {
432    pub index: u16,
433    pub sub_index: u8,
434    pub payload: SdoCmdInitiatePayload,
435}
436
437impl SdoCmdInitiateUploadTx {
438    fn encode(&self, frame: &mut socketcan::CanFrame) {
439        let mut data = [0u8; 8];
440        let command_byte = 0b010 << 5;
441        data[0] = command_byte;
442        data[1..3].copy_from_slice(&self.index.to_le_bytes());
443        data[3] = self.sub_index;
444        frame.set_data(&data).unwrap();
445        self.payload.encode(frame);
446    }
447
448    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
449        let index = u16::from_le_bytes(
450            frame.data()[1..3]
451                .try_into()
452                .map_err(|_| CanOpenError::ParseError("not enough data".to_owned()))?,
453        );
454        let sub_index = frame.data()[3];
455        let payload = SdoCmdInitiatePayload::decode(frame)?;
456        Ok(Self {
457            index,
458            sub_index,
459            payload,
460        })
461    }
462}
463
464#[derive(Clone, Debug)]
465pub struct SdoCmdUploadSegmentRx {
466    pub toggle: bool,
467}
468
469impl SdoCmdUploadSegmentRx {
470    fn encode(&self, frame: &mut socketcan::CanFrame) {
471        let mut data = [0u8; 8];
472        data[0] = 0b011 << 5 | ((self.toggle as u8) << 4);
473        frame.set_data(&data).unwrap();
474    }
475
476    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
477        let command_byte = frame.data()[0];
478        let toggle = command_byte & 0b10000 != 0;
479        Ok(Self { toggle })
480    }
481}
482
483#[derive(Clone, Debug)]
484pub struct SdoCmdUploadSegmentTx {
485    pub toggle: bool,
486    pub data: Box<[u8]>,
487    pub last: bool,
488}
489
490impl SdoCmdUploadSegmentTx {
491    fn encode(&self, frame: &mut socketcan::CanFrame) {
492        let mut data = [0u8; 8];
493        data[0] =
494            ((self.toggle as u8) << 4) | ((7 - self.data.len() as u8) << 1) | (self.last as u8);
495        data[1..1 + self.data.len()].copy_from_slice(&self.data);
496        frame.set_data(&data).unwrap();
497    }
498
499    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
500        let command_byte = frame.data()[0];
501        let toggle = command_byte & 0b10000 != 0;
502        let size = 7 - (0b111 & (command_byte >> 1)) as usize;
503        let last = command_byte & 0b1 != 0;
504        let mut data = Vec::new();
505        data.extend_from_slice(&frame.data()[1..1 + size]);
506
507        Ok(Self {
508            toggle,
509            last,
510            data: data.into(),
511        })
512    }
513}
514
515#[derive(Clone, Debug)]
516pub struct SdoCmdAbortTransfer {
517    pub index: u16,
518    pub sub_index: u8,
519    // TODO: translate abort codes from CIA301 page 61 into a thiserror enum
520    pub abort_code: enums::AbortCode,
521}
522
523impl SdoCmdAbortTransfer {
524    fn encode(&self, frame: &mut socketcan::CanFrame) {
525        let mut data = [0u8; 8];
526        let command_byte = 0b100 << 5;
527        data[0] = command_byte;
528        data[1..3].copy_from_slice(&self.index.to_le_bytes());
529        data[3] = self.sub_index;
530        data[4..8].copy_from_slice(&self.abort_code.encode().to_le_bytes());
531        frame.set_data(&data).unwrap();
532    }
533
534    fn decode(frame: &socketcan::CanFrame) -> Result<Self, CanOpenError> {
535        let index = u16::from_le_bytes(
536            frame.data()[1..3]
537                .try_into()
538                .map_err(|_| CanOpenError::ParseError("not enough data".to_owned()))?,
539        );
540        let sub_index = frame.data()[3];
541        let abort_code_u32 = u32::from_le_bytes(frame.data()[4..8].try_into().unwrap());
542        let abort_code = enums::AbortCode::decode(abort_code_u32)
543            .ok_or_else(|| CanOpenError::ParseError(format!("invalid abort code: {abort_code_u32}")))?;
544
545        Ok(Self {
546            index,
547            sub_index,
548            abort_code,
549        })
550    }
551}
552
553/// SDO Command Specifier
554/// SDOs let you read/write object dictionary keys
555/// An expedited SDO message carries at most 4 bytes
556/// Segmented SDOs are for sending more than 4 bytes
557/// As usual in embedded, comms are from the perspective of the embedded device,
558/// so download = client to server, upload = server to client.
559///
560/// When writing less than 4 bytes, you send a `InitiateDownload` RX,
561/// the device ACKs it with `InitiateDownload` Res
562///
563/// When writing more than 4 bytes, you send an `InitiateDownload` RX
564/// with the "segmented" flag set and the # of bytes you'll send in the data field.
565/// The device still acks this with `InitiateDownload` Res.
566/// After this, you send each segment with a `DownloadSegment`. The device still ACKs each segment.
567/// The `DownloadSegment` Res can carry at most 8 bytes
568///
569/// Reading works symmetrically to writing
570#[derive(Clone, Debug, PartialEq)]
571enum SdoCmdSpec {
572    DownloadSegment,
573    InitiateDownload,
574    InitiateUpload,
575    UploadSegment,
576    AbortTransfer,
577    BlockUpload,
578    BlockDownload,
579}
580
581impl SdoCmdSpec {
582    pub fn from_byte(byte: u8, reqres: ReqRes) -> Result<SdoCmdSpec, CanOpenError> {
583        use SdoCmdSpec::*;
584        let v = match (reqres, byte >> 5) {
585            // meaning of the command specifier is slightly different for Rx and Tx
586            // thx: https://github.com/wireshark/wireshark/blob/master/epan/dissectors/packet-canopen.c#L511
587            (ReqRes::Req, 0x00) => DownloadSegment,
588            (ReqRes::Req, 0x01) => InitiateDownload,
589            (ReqRes::Req, 0x02) => InitiateUpload,
590            (ReqRes::Req, 0x03) => UploadSegment,
591
592            (ReqRes::Res, 0x00) => UploadSegment,
593            (ReqRes::Res, 0x01) => DownloadSegment,
594            (ReqRes::Res, 0x02) => InitiateUpload,
595            (ReqRes::Res, 0x03) => InitiateDownload,
596
597            (_, 0x04) => AbortTransfer,
598            (_, 0x05) => BlockUpload,
599            (_, 0x06) => BlockDownload,
600            _ => {
601                return Err(CanOpenError::ParseError(format!(
602                    "bad client command specifier: {}",
603                    byte
604                )))
605            }
606        };
607        Ok(v)
608    }
609}
610
611impl Sdo {
612    pub fn new_write(node_id: u8, index: u16, sub_index: u8, data: Box<[u8]>) -> Sdo {
613        Sdo {
614            node_id,
615            reqres: ReqRes::Req,
616            command: SdoCmd::InitiateDownloadRx(SdoCmdInitiateDownloadRx {
617                index,
618                sub_index,
619                payload: SdoCmdInitiatePayload::Expedited(data),
620            }),
621        }
622    }
623    pub fn new_write_resp(node_id: u8, index: u16, sub_index: u8) -> Sdo {
624        Sdo {
625            node_id,
626            reqres: ReqRes::Res,
627            command: SdoCmd::InitiateDownloadTx(SdoCmdInitiateDownloadTx { index, sub_index }),
628        }
629    }
630}
631
632impl FrameRW for Sdo {
633    fn decode(frame: &socketcan::CanFrame) -> Result<Sdo, CanOpenError> {
634        let data = frame.data();
635
636        let id = id_as_raw_std(frame)?;
637        if !(0x580..=0x5FF).contains(&id) && !(0x600..=0x67F).contains(&id) {
638            return Err(CanOpenError::BadMessage(format!(
639                "{id} is not an SDO can id"
640            ))); // Not a valid SDO COB-ID
641        }
642
643        let node_id = (id & 0x7F) as u8;
644        let reqres = ReqRes::from_u16_sdo(id);
645
646        let command_spec = SdoCmdSpec::from_byte(data[0], reqres)?;
647        let command = match (reqres, command_spec) {
648            (ReqRes::Req, SdoCmdSpec::InitiateDownload) => {
649                SdoCmd::InitiateDownloadRx(SdoCmdInitiateDownloadRx::decode(frame)?)
650            }
651            (ReqRes::Req, SdoCmdSpec::DownloadSegment) => {
652                SdoCmd::DownloadSegmentRx(SdoCmdDownloadSegmentRx::decode(frame)?)
653            }
654            (ReqRes::Res, SdoCmdSpec::InitiateDownload) => {
655                SdoCmd::InitiateDownloadTx(SdoCmdInitiateDownloadTx::decode(frame)?)
656            }
657            (ReqRes::Res, SdoCmdSpec::DownloadSegment) => {
658                SdoCmd::DownloadSegmentTx(SdoCmdDownloadSegmentTx::decode(frame)?)
659            }
660            (ReqRes::Req, SdoCmdSpec::InitiateUpload) => {
661                SdoCmd::InitiateUploadRx(SdoCmdInitiateUploadRx::decode(frame)?)
662            }
663            (ReqRes::Req, SdoCmdSpec::UploadSegment) => {
664                SdoCmd::UploadSegmentRx(SdoCmdUploadSegmentRx::decode(frame)?)
665            }
666            (ReqRes::Res, SdoCmdSpec::InitiateUpload) => {
667                SdoCmd::InitiateUploadTx(SdoCmdInitiateUploadTx::decode(frame)?)
668            }
669            (ReqRes::Res, SdoCmdSpec::UploadSegment) => {
670                SdoCmd::UploadSegmentTx(SdoCmdUploadSegmentTx::decode(frame)?)
671            }
672            (_, SdoCmdSpec::AbortTransfer) => {
673                SdoCmd::AbortTransfer(SdoCmdAbortTransfer::decode(frame)?)
674            }
675            _ => return Err(CanOpenError::NotYetImplemented("block transfer".to_owned())),
676        };
677        let sdo = Sdo {
678            node_id,
679            command,
680            reqres,
681        };
682        Ok(sdo)
683    }
684
685    fn encode(&self, frame: &mut socketcan::CanFrame) {
686        frame.set_id(u16_as_id((self.node_id as u16) + self.reqres.to_u16_sdo()));
687        match &self.command {
688            SdoCmd::InitiateUploadRx(inner) => inner.encode(frame),
689            SdoCmd::InitiateDownloadRx(inner) => inner.encode(frame),
690            SdoCmd::UploadSegmentRx(inner) => inner.encode(frame),
691            SdoCmd::DownloadSegmentRx(inner) => inner.encode(frame),
692            SdoCmd::InitiateUploadTx(inner) => inner.encode(frame),
693            SdoCmd::InitiateDownloadTx(inner) => inner.encode(frame),
694            SdoCmd::UploadSegmentTx(inner) => inner.encode(frame),
695            SdoCmd::DownloadSegmentTx(inner) => inner.encode(frame),
696            SdoCmd::AbortTransfer(inner) => inner.encode(frame),
697            _ => todo!(),
698        };
699    }
700}
701
702#[derive(Debug, Copy, Clone)]
703#[repr(u8)]
704pub enum GuardStatus {
705    Boot = 0x00,
706    Stopped = 0x04,
707    Operational = 0x05,
708    PreOperational = 0x7F,
709}
710
711impl TryFrom<u8> for GuardStatus {
712    type Error = String;
713    fn try_from(value: u8) -> Result<Self, Self::Error> {
714        match value {
715            0x00 => Ok(GuardStatus::Boot),
716            0x04 => Ok(GuardStatus::Stopped),
717            0x05 => Ok(GuardStatus::Operational),
718            0x7F => Ok(GuardStatus::PreOperational),
719            _ => Err(format!("{value:x} not a valid guard status")),
720        }
721    }
722}
723
724#[binrw]
725#[brw(little)]
726#[derive(Debug)]
727pub struct Guard {
728    #[brw(ignore)]
729    node_id: u8,
730
731    #[br(temp)]
732    #[bw(calc = (*status as u8)  | ((*toggle as u8) << 7))]
733    raw_byte: u8,
734
735    #[br(calc = raw_byte & 0x80 != 0)]
736    #[bw(ignore)]
737    toggle: bool,
738
739    #[br(try_calc = (raw_byte & 0x7F).try_into())]
740    #[bw(ignore)]
741    status: GuardStatus,
742}
743
744impl Guard {
745    pub fn new(node_id: u8, toggle: bool, status: GuardStatus) -> Self {
746        Self {
747            node_id,
748            toggle,
749            status,
750        }
751    }
752}
753
754impl FrameRW for Guard {
755    fn decode(frame: &socketcan::CanFrame) -> Result<Guard, CanOpenError> {
756        let data = frame.data();
757        if data.is_empty() {
758            return Err(CanOpenError::ParseError("data too short".to_owned()));
759        }
760
761        let id = id_as_raw_std(frame)?;
762        if !(0x700..=0x77F).contains(&id) {
763            return Err(CanOpenError::BadMessage("wrong id".to_owned()));
764        }
765        Guard::read(&mut std::io::Cursor::new(&data))
766            .map_err(|e| CanOpenError::ParseError(format!("no parse: {e}")))
767    }
768
769    fn encode(&self, frame: &mut socketcan::CanFrame) {
770        frame.set_id(u16_as_id(0x700 + self.node_id as u16));
771        let mut c = std::io::Cursor::new(Vec::new());
772        self.write(&mut c).unwrap();
773        frame.set_data(c.get_ref()).unwrap();
774    }
775}
776
777/// ReqRes realtive to the device (aka server)
778/// Data sent from your computer is probably Rx,
779/// since the device is receiving it)
780#[derive(Default, Debug, Clone, Copy, PartialEq, Eq)]
781pub enum ReqRes {
782    #[default]
783    Req,
784    Res,
785}
786impl ReqRes {
787    pub fn to_u16_sdo(&self) -> u16 {
788        match self {
789            ReqRes::Req => 0x600,
790            ReqRes::Res => 0x580,
791        }
792    }
793    // TODO: SDO can go over other CAN IDs
794    // (page 126 of cia301)
795    // this just supports the default one
796    // as I have not seen other SDOs in the wild
797    pub fn from_u16_sdo(id: u16) -> Self {
798        if id & 0x780 == 0x580 {
799            ReqRes::Res
800        } else {
801            ReqRes::Req
802        }
803    }
804}
805
806#[derive(Debug)]
807pub struct Pdo {
808    node_id: u8,
809    pdo_index: u8, // PDO index (1 to 4)
810    reqres: ReqRes,
811    data: Vec<u8>, // Data (1 to 8 bytes)
812}
813
814impl Pdo {
815    pub fn new(node_id: u8, pdo_index: u8, data: &[u8]) -> Result<Self, CanOpenError> {
816        if !(1..=8).contains(&data.len()) {
817            return Err(CanOpenError::BadMessage(format!(
818                "got {} bytes of PDO data, expected between 1 and 8 bytes",
819                data.len()
820            )));
821        }
822        Ok(Self {
823            node_id,
824            pdo_index,
825            reqres: ReqRes::Req,
826            data: data.to_owned(),
827        })
828    }
829}
830
831impl FrameRW for Pdo {
832    fn decode(frame: &socketcan::CanFrame) -> Result<Pdo, CanOpenError> {
833        let id = id_as_raw_std(frame)?;
834        let data = frame.data().to_vec();
835
836        // Determine RX/Res and PDO index from the COB-ID
837        let reqres = if id & 0x80 == 0 { ReqRes::Res } else { ReqRes::Req };
838
839        // this is a bit odd, RX indicies are offset by one
840        let pdo_index = (((id & 0x700) >> 8) as u8) - if reqres == ReqRes::Req { 1u8 } else { 0u8 };
841
842        let node_id = (id & 0x7F) as u8;
843
844        Ok(Pdo {
845            pdo_index,
846            reqres,
847            node_id,
848            data,
849        })
850    }
851
852    fn encode(&self, frame: &mut socketcan::CanFrame) {
853        let id = (self.pdo_index as u16 + if self.reqres == ReqRes::Req { 1 } else { 0 }) << 8;
854        frame.set_id(u16_as_id(self.node_id as u16 + id));
855        // unwrap wont panic here, we guarantee data is between 1 and 8 bytes
856        frame.set_data(&self.data).unwrap();
857    }
858}
859
860#[derive(Debug)]
861/// Sync messages are usually sent at regular intervals.
862/// "Synchronous events" are often driven by sync messages.
863/// For example, you can configure PDOs to be sent after every sync message.
864pub struct Sync;
865
866impl FrameRW for Sync {
867    fn decode(frame: &socketcan::CanFrame) -> Result<Sync, CanOpenError> {
868        let id = id_as_raw_std(frame)?;
869        if id != 0x80 {
870            Err(CanOpenError::BadMessage(format!("not a SYNC cob-id: {id}")))
871        } else if !frame.data().is_empty() {
872            Err(CanOpenError::BadMessage(format!(
873                "data section of SYNC message should be empty, found {} bytes",
874                frame.data().len()
875            )))
876        } else {
877            Ok(Sync {})
878        }
879    }
880
881    fn encode(&self, frame: &mut socketcan::CanFrame) {
882        frame.set_id(u16_as_id(0x80));
883        frame.set_data(&[]).unwrap();
884    }
885}
886
887#[derive(Debug)]
888pub enum Message {
889    Nmt(Nmt),
890    Sync(Sync),
891    Emergency(Emergency),
892    Pdo(Pdo),
893    Sdo(Sdo),
894    Guard(Guard),
895}
896
897use thiserror::Error;
898
899#[derive(Error, Debug)]
900pub enum CanOpenError {
901    #[error("Overflow error: {0}")]
902    OverflowError(String),
903
904    #[error("Timed out after {0} ms")]
905    Timeout(u64),
906
907    #[error("FrameRW protocl is not {0}")]
908    BadMessage(String),
909
910    #[error("Connection error: {0}")]
911    ConnectionError(String),
912
913    #[error("CAN version mismatch: {0}")]
914    CanVersion(String),
915
916    #[error("Parse error: {0}")]
917    ParseError(String),
918
919    #[error("Unknown message type with COB-ID: {0}")]
920    UnknownFrameRWType(u32),
921
922    #[error("Not yet implemented: {0}")]
923    NotYetImplemented(String),
924
925    #[error("SDO AbortTransfer error, abort code: {0:?}")]
926    SdoAbortTransfer(enums::AbortCode),
927
928    #[error("IO Error: {0}")]
929    IOError(std::io::Error),
930}
931
932/// CAN connection. Connects on `Conn::new()`
933/// Writing/reading a single CAN frame is thread safe,
934/// since socketcan guarantees atomic frame reads and writes.
935/// Multi-frame operations (eg send_acked or sdo_read) are not thread safe
936/// as they rely on receiving multiple can frames.
937#[derive(Debug)]
938pub struct Conn {
939    socket: socketcan::CanSocket,
940}
941
942impl Conn {
943    pub fn new(interface_name: &str) -> Result<Self, CanOpenError> {
944        let socket = socketcan::CanSocket::open(interface_name).expect("no iface");
945        Ok(Conn { socket })
946    }
947
948    pub fn recv(&self) -> Result<Message, CanOpenError> {
949        let frame = self.socket.read_frame().map_err(CanOpenError::IOError)?;
950        Self::decode(&frame)
951    }
952
953    pub fn set_read_timeout(&self, t: std::time::Duration) -> Result<(), CanOpenError> {
954        self.socket
955            .set_read_timeout(t)
956            .map_err(CanOpenError::IOError)
957    }
958
959    pub fn set_write_timeout(&self, t: std::time::Duration) -> Result<(), CanOpenError> {
960        self.socket
961            .set_write_timeout(t)
962            .map_err(CanOpenError::IOError)
963    }
964
965    fn send_sdo_acked(&self, message: Sdo, node_id: u8) -> Result<Sdo, CanOpenError> {
966        self.send(&Message::Sdo(message.clone()))?;
967        loop {
968            let resp = self.recv()?;
969            if Self::is_sdo_ack(&resp, &message.command, node_id)? {
970                match resp {
971                    Message::Sdo(sdo) => return Ok(sdo),
972                    _ => unreachable!(),
973                }
974            };
975        }
976    }
977
978    fn is_sdo_ack(message: &Message, command: &SdoCmd, node_id: u8) -> Result<bool, CanOpenError> {
979        match message {
980            Message::Sdo(sdo) if sdo.node_id == node_id => match &sdo.command {
981                SdoCmd::AbortTransfer(e) => Err(CanOpenError::SdoAbortTransfer(e.abort_code.clone())),
982                cmd if SdoCmd::is_response_to(command, cmd) => Ok(true),
983                _ => Ok(false),
984            },
985            _ => Ok(false),
986        }
987    }
988
989    pub fn sdo_write(
990        &mut self,
991        node_id: u8,
992        index: u16,
993        sub_index: u8,
994        data: &[u8],
995    ) -> Result<(), CanOpenError> {
996        match data.len() {
997            // 0 bytes - nothing to do
998            0 => Ok(()),
999            // <= 4 bytes - single expedited write
1000            1..=4 => {
1001                let message = Sdo {
1002                    node_id,
1003                    reqres: ReqRes::Req,
1004                    command: SdoCmd::InitiateDownloadRx(SdoCmdInitiateDownloadRx {
1005                        index,
1006                        sub_index,
1007                        payload: SdoCmdInitiatePayload::Expedited(data.into()),
1008                    }),
1009                };
1010                self.send_sdo_acked(message, node_id)?;
1011                Ok(())
1012            }
1013            // > 4 bytes - segmented write
1014            n => {
1015                let mut toggle = false;
1016                let init_message = Sdo {
1017                    node_id,
1018                    reqres: ReqRes::Req,
1019                    command: SdoCmd::InitiateDownloadRx(SdoCmdInitiateDownloadRx {
1020                        index,
1021                        sub_index,
1022                        payload: SdoCmdInitiatePayload::Segmented(Some(
1023                            data.len().try_into().map_err(|e: TryFromIntError| {
1024                                CanOpenError::OverflowError(e.to_string())
1025                            })?,
1026                        )),
1027                    }),
1028                };
1029                self.send_sdo_acked(init_message, node_id)?;
1030
1031                for (idx_seg_start, _) in data.iter().enumerate().step_by(7) {
1032                    let idx_seg_end = std::cmp::min(idx_seg_start + 7, n);
1033                    let last = idx_seg_start + 7 >= n;
1034                    let message = Sdo {
1035                        node_id,
1036                        reqres: ReqRes::Req,
1037                        command: SdoCmd::DownloadSegmentRx(SdoCmdDownloadSegmentRx {
1038                            toggle,
1039                            data: data[idx_seg_start..idx_seg_end].into(),
1040                            last,
1041                        }),
1042                    };
1043                    toggle = !toggle;
1044                    self.send_sdo_acked(message, node_id)?;
1045                }
1046                Ok(())
1047            }
1048        }
1049    }
1050
1051    pub fn sdo_read(
1052        &mut self,
1053        node_id: u8,
1054        index: u16,
1055        sub_index: u8,
1056    ) -> Result<Box<[u8]>, CanOpenError> {
1057        let res = self.send_sdo_acked(
1058            Sdo {
1059                node_id,
1060                command: SdoCmd::InitiateUploadRx(SdoCmdInitiateUploadRx { index, sub_index }),
1061                reqres: ReqRes::Req,
1062            },
1063            node_id,
1064        )?;
1065
1066        match res.command {
1067            SdoCmd::InitiateUploadTx(SdoCmdInitiateUploadTx {
1068                index: _,
1069                sub_index: _,
1070                payload: SdoCmdInitiatePayload::Expedited(data),
1071            }) => Ok(data),
1072            SdoCmd::InitiateUploadTx(SdoCmdInitiateUploadTx {
1073                index: _,
1074                sub_index: _,
1075                payload: SdoCmdInitiatePayload::Segmented(maybe_len),
1076            }) => {
1077                let mut buffer = Vec::new();
1078                let mut toggle = false;
1079                if let Some(len) = maybe_len {
1080                    buffer.reserve(len as usize);
1081                };
1082                loop {
1083                    let seg = self.send_sdo_acked(
1084                        Sdo {
1085                            reqres: ReqRes::Req,
1086                            node_id,
1087                            command: SdoCmd::UploadSegmentRx(SdoCmdUploadSegmentRx { toggle }),
1088                        },
1089                        node_id,
1090                    )?;
1091                    if let Sdo {
1092                        reqres: _,
1093                        node_id: _,
1094                        command: SdoCmd::UploadSegmentTx(command),
1095                    } = seg
1096                    {
1097                        buffer.extend_from_slice(&command.data);
1098                        if command.last {
1099                            break;
1100                        }
1101                    }
1102                    toggle = !toggle;
1103                }
1104                Ok(buffer.into())
1105            }
1106            _ => unreachable!(),
1107        }
1108    }
1109
1110    pub fn send(&self, message: &Message) -> Result<(), CanOpenError> {
1111        let mut frame = socketcan::CanFrame::new(
1112            socketcan::Id::Standard(socketcan::StandardId::new(0).unwrap()),
1113            &[],
1114        )
1115        .unwrap();
1116        match message {
1117            Message::Sdo(sdo) => sdo.encode(&mut frame),
1118            Message::Pdo(pdo) => pdo.encode(&mut frame),
1119            Message::Sync(sync) => sync.encode(&mut frame),
1120            Message::Nmt(nmt) => nmt.encode(&mut frame),
1121            Message::Emergency(emergency) => emergency.encode(&mut frame),
1122            Message::Guard(guard) => guard.encode(&mut frame),
1123        }
1124        self.socket
1125            .write_frame(&frame)
1126            .map_err(CanOpenError::IOError)
1127    }
1128
1129    fn decode(frame: &socketcan::CanFrame) -> Result<Message, CanOpenError> {
1130        let id = id_as_raw_std(frame).unwrap();
1131        // can_id is node_id + protocol_id (same as function id)
1132        // can_ids are always <128
1133        // mask out lowest 7 bits to just get the protocol_id
1134        let protocol_id = id & 0xFF80;
1135        // apply the opposite mask for node_id
1136        let node_id = id & 0x007F;
1137        let p = match protocol_id {
1138            0x000 => Message::Nmt(Nmt::decode(frame)?),
1139            0x080 if node_id == 0 => Message::Sync(Sync::decode(frame)?),
1140            0x080 => Message::Emergency(Emergency::decode(frame)?),
1141            0x180..=0x500 => Message::Pdo(Pdo::decode(frame)?),
1142            0x580..=0x600 => Message::Sdo(Sdo::decode(frame)?),
1143            0x700 => Message::Guard(Guard::decode(frame)?),
1144            _ => todo!(),
1145        };
1146        Ok(p)
1147    }
1148}
canopeners/lib.rs

canopeners/
lib.rs
