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