socketcan_rs/
frame.rs

1use crate::{socket, FD_FRAME_SIZE, FRAME_SIZE, XL_FRAME_SIZE};
2use libc::{can_frame, canfd_frame, canxl_frame};
3use rs_can::{
4    can_utils, CanDirection, CanError, CanFdFlags, CanFrame, CanId, CanKind, CanResult,
5    FrameFormat, IdentifierFlags, Timestamp, EFF_MASK, MAX_FD_FRAME_SIZE, MAX_FRAME_SIZE,
6};
7use std::fmt::{Display, Formatter};
8
9pub enum CanAnyFrame {
10    Normal(can_frame),
11    Remote(can_frame),
12    Error(can_frame),
13    FD(canfd_frame),
14    XL(canxl_frame),
15}
16
17impl CanAnyFrame {
18    pub fn size(&self) -> usize {
19        match self {
20            Self::Normal(_) => FRAME_SIZE,
21            Self::Remote(_) => FRAME_SIZE,
22            Self::Error(_) => FRAME_SIZE,
23            Self::FD(_) => FD_FRAME_SIZE,
24            Self::XL(_) => XL_FRAME_SIZE,
25        }
26    }
27}
28
29impl From<can_frame> for CanAnyFrame {
30    #[inline(always)]
31    fn from(frame: can_frame) -> CanAnyFrame {
32        let can_id = frame.can_id;
33        if can_id & IdentifierFlags::REMOTE.bits() != 0 {
34            Self::Remote(frame)
35        } else if can_id & IdentifierFlags::ERROR.bits() != 0 {
36            Self::Error(frame)
37        } else {
38            Self::Normal(frame)
39        }
40    }
41}
42
43impl From<canfd_frame> for CanAnyFrame {
44    #[inline(always)]
45    fn from(frame: canfd_frame) -> Self {
46        Self::FD(frame)
47    }
48}
49
50impl From<canxl_frame> for CanAnyFrame {
51    fn from(frame: canxl_frame) -> Self {
52        Self::XL(frame)
53    }
54}
55
56#[derive(Debug, Clone)]
57pub struct SocketCanFrame {
58    pub(crate) timestamp: Option<Timestamp>,
59    pub(crate) arbitration_id: u32,
60    pub(crate) is_extended_id: bool,
61    pub(crate) is_remote_frame: bool,
62    pub(crate) is_error_frame: bool,
63    pub(crate) channel: String,
64    pub(crate) length: usize,
65    pub(crate) data: Vec<u8>,
66    pub(crate) kind: CanKind,
67    pub(crate) direction: CanDirection,
68    pub(crate) bitrate_switch: bool,
69    pub(crate) error_state_indicator: bool,
70}
71
72impl SocketCanFrame {
73    fn socketcan_id_bits(&self) -> u32 {
74        let id = self.id();
75        let mut can_id = id.into_socketcan_bits();
76
77        if self.is_error_frame {
78            can_id |= IdentifierFlags::ERROR.bits();
79        }
80
81        if self.is_remote_frame {
82            can_id |= IdentifierFlags::REMOTE.bits();
83        }
84
85        can_id
86    }
87}
88
89impl TryFrom<CanAnyFrame> for SocketCanFrame {
90    type Error = CanError;
91
92    fn try_from(frame: CanAnyFrame) -> Result<Self, Self::Error> {
93        match frame {
94            CanAnyFrame::Normal(f) => Ok(Self {
95                timestamp: None,
96                arbitration_id: f.can_id & EFF_MASK,
97                is_extended_id: f.can_id & IdentifierFlags::EXTENDED.bits() != 0,
98                is_remote_frame: false,
99                is_error_frame: false,
100                channel: Default::default(),
101                length: f.can_dlc as usize,
102                data: f.data[..f.can_dlc as usize].to_vec(),
103                kind: CanKind::Classical,
104                direction: Default::default(),
105                bitrate_switch: false,
106                error_state_indicator: false,
107            }),
108            CanAnyFrame::Remote(f) => Ok(Self {
109                timestamp: None,
110                arbitration_id: f.can_id & EFF_MASK,
111                is_extended_id: f.can_id & IdentifierFlags::EXTENDED.bits() != 0,
112                is_remote_frame: true,
113                is_error_frame: false,
114                channel: Default::default(),
115                length: f.can_dlc as usize,
116                data: f.data[..f.can_dlc as usize].to_vec(),
117                kind: CanKind::Classical,
118                direction: Default::default(),
119                bitrate_switch: false,
120                error_state_indicator: false,
121            }),
122            CanAnyFrame::Error(f) => Ok(Self {
123                timestamp: None,
124                arbitration_id: f.can_id & EFF_MASK,
125                is_extended_id: f.can_id & IdentifierFlags::EXTENDED.bits() != 0,
126                is_remote_frame: false,
127                is_error_frame: true,
128                channel: Default::default(),
129                length: f.can_dlc as usize,
130                data: f.data[..f.can_dlc as usize].to_vec(),
131                kind: CanKind::Classical,
132                direction: Default::default(),
133                bitrate_switch: false,
134                error_state_indicator: false,
135            }),
136            CanAnyFrame::FD(f) => Ok(Self {
137                timestamp: None,
138                arbitration_id: f.can_id & EFF_MASK,
139                is_extended_id: f.can_id & IdentifierFlags::EXTENDED.bits() != 0,
140                is_remote_frame: false,
141                is_error_frame: false,
142                channel: Default::default(),
143                length: f.len as usize,
144                data: f.data[..f.len as usize].to_vec(),
145                kind: CanKind::FD,
146                direction: Default::default(),
147                bitrate_switch: f.flags & 0x01 != 0,
148                error_state_indicator: f.flags & 0x02 != 0,
149            }),
150            CanAnyFrame::XL(_) => Err(CanError::NotImplementedError),
151        }
152    }
153}
154
155impl Into<CanAnyFrame> for SocketCanFrame {
156    fn into(self) -> CanAnyFrame {
157        match self.kind {
158            CanKind::Classical => {
159                let mut frame = socket::can_frame_default();
160                let length = self.data.len();
161                frame.data[..length].copy_from_slice(&self.data);
162                frame.can_dlc = length as u8;
163                let can_id = self.socketcan_id_bits();
164
165                if self.is_error_frame {
166                    frame.can_id = can_id;
167                    return CanAnyFrame::Error(frame);
168                }
169
170                if self.is_remote_frame {
171                    frame.can_id = can_id;
172                    return CanAnyFrame::Remote(frame);
173                }
174
175                frame.can_id = can_id;
176                CanAnyFrame::Normal(frame)
177            }
178            CanKind::FD => {
179                let mut frame = socket::canfd_frame_default();
180                let can_id = self.socketcan_id_bits();
181
182                let length = self.data.len();
183                frame.can_id = can_id;
184                frame.data[..length].copy_from_slice(&self.data);
185                frame.len = length as u8;
186                frame.flags |= CanFdFlags::FDF.bits();
187                if self.bitrate_switch {
188                    frame.flags |= CanFdFlags::BRS.bits();
189                }
190
191                if self.error_state_indicator {
192                    frame.flags |= CanFdFlags::ESI.bits();
193                }
194
195                CanAnyFrame::FD(frame)
196            }
197            CanKind::XL => todo!("XL is not supported now!"),
198        }
199    }
200}
201
202impl CanFrame for SocketCanFrame {
203    type Channel = String;
204
205    fn new_can(id: CanId, data: &[u8]) -> CanResult<Self> {
206        let length = data.len();
207        if length > MAX_FRAME_SIZE {
208            return Err(CanError::InvalidDLC(length));
209        }
210
211        Ok(Self {
212            timestamp: None,
213            arbitration_id: id.as_raw(),
214            is_extended_id: id.is_extended(),
215            is_remote_frame: false,
216            is_error_frame: false,
217            channel: Default::default(),
218            length,
219            data: data.to_vec(),
220            kind: CanKind::Classical,
221            direction: Default::default(),
222            bitrate_switch: false,
223            error_state_indicator: false,
224        })
225    }
226
227    fn new_remote(id: CanId, dlc: u8) -> CanResult<Self> {
228        let dlc = dlc as usize;
229        if dlc > MAX_FRAME_SIZE {
230            return Err(CanError::InvalidDLC(dlc));
231        }
232        let mut data = Vec::new();
233        can_utils::data_resize(&mut data, dlc);
234
235        Ok(Self {
236            timestamp: None,
237            arbitration_id: id.as_raw(),
238            is_extended_id: id.is_extended(),
239            is_remote_frame: true,
240            is_error_frame: false,
241            channel: Default::default(),
242            length: dlc,
243            data,
244            kind: CanKind::Classical,
245            direction: Default::default(),
246            bitrate_switch: false,
247            error_state_indicator: false,
248        })
249    }
250
251    fn new_can_fd(id: CanId, data: &[u8], flags: CanFdFlags) -> CanResult<Self> {
252        let length = data.len();
253        if length > MAX_FD_FRAME_SIZE {
254            return Err(CanError::InvalidDLC(length));
255        }
256
257        Ok(Self {
258            timestamp: None,
259            arbitration_id: id.as_raw(),
260            is_extended_id: id.is_extended(),
261            is_remote_frame: false,
262            is_error_frame: false,
263            channel: Default::default(),
264            length,
265            data: data.to_vec(),
266            kind: CanKind::FD,
267            direction: Default::default(),
268            bitrate_switch: flags.contains(CanFdFlags::BRS),
269            error_state_indicator: flags.contains(CanFdFlags::ESI),
270        })
271    }
272
273    fn id(&self) -> CanId {
274        CanId::from_bits(self.arbitration_id, Some(self.is_extended_id)).unwrap()
275    }
276
277    fn channel(&self) -> Self::Channel {
278        self.channel.clone()
279    }
280
281    fn set_channel(&mut self, v: Self::Channel) -> &mut Self {
282        self.channel = v;
283        self
284    }
285
286    fn kind(&self) -> CanKind {
287        self.kind
288    }
289
290    fn format(&self) -> FrameFormat {
291        if self.is_error_frame {
292            FrameFormat::Error
293        } else if self.is_remote_frame {
294            FrameFormat::Remote
295        } else {
296            FrameFormat::Data
297        }
298    }
299
300    fn data(&self) -> &[u8] {
301        &self.data
302    }
303
304    fn len(&self) -> usize {
305        self.length
306    }
307
308    fn direction(&self) -> CanDirection {
309        self.direction
310    }
311
312    fn set_direction(&mut self, d: CanDirection) -> &mut Self {
313        self.direction = d;
314        self
315    }
316
317    fn timestamp(&self) -> Option<Timestamp> {
318        self.timestamp
319    }
320
321    fn set_timestamp(&mut self, ts: Option<Timestamp>) -> &mut Self {
322        self.timestamp = ts;
323        self
324    }
325
326    fn is_bitrate_switch(&self) -> bool {
327        self.bitrate_switch
328    }
329
330    fn set_bitrate_switch(&mut self, v: bool) -> &mut Self {
331        self.bitrate_switch = v;
332        self
333    }
334
335    fn is_esi(&self) -> bool {
336        self.error_state_indicator
337    }
338
339    fn set_esi(&mut self, v: bool) -> &mut Self {
340        self.error_state_indicator = v;
341        self
342    }
343}
344
345impl PartialEq for SocketCanFrame {
346    fn eq(&self, other: &Self) -> bool {
347        if self.length != other.length {
348            return false;
349        }
350
351        if self.is_remote_frame {
352            other.is_remote_frame && (self.arbitration_id == other.arbitration_id)
353        } else {
354            (self.arbitration_id == other.arbitration_id)
355                && (self.is_extended_id == other.is_extended_id)
356                && (self.is_error_frame == other.is_error_frame)
357                && (self.error_state_indicator == other.error_state_indicator)
358                && (self.data == other.data)
359        }
360    }
361}
362
363impl Display for SocketCanFrame {
364    fn fmt(&self, f: &mut Formatter<'_>) -> std::fmt::Result {
365        <dyn CanFrame<Channel = String> as Display>::fmt(self, f)
366    }
367}
socketcan_rs/frame.rs

socketcan_rs/
frame.rs
