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