1// Note: This file is copied and modified from fdcan crate by Richard Meadows
2
3use core::convert::Infallible;
4use core::slice;
5
6use cfg_if::cfg_if;
7
8use crate::can::enums::*;
9use crate::can::fd::config::*;
10use crate::can::fd::message_ram::enums::*;
11use crate::can::fd::message_ram::{RegisterBlock, RxFifoElement, TxBufferElement};
12use crate::can::frame::*;
13
14/// Loopback Mode
15#[derive(Clone, Copy, Debug)]
16enum LoopbackMode {
17 None,
18 Internal,
19 External,
20}
21
22pub struct Registers {
23 pub regs: crate::pac::can::Fdcan,
24 pub msgram: crate::pac::fdcanram::Fdcanram,
25 #[allow(dead_code)]
26 pub msg_ram_offset: usize,
27}
28
29impl Registers {
30 fn tx_buffer_element(&self, bufidx: usize) -> &mut TxBufferElement {
31 &mut self.msg_ram_mut().transmit.tbsa[bufidx]
32 }
33 pub fn msg_ram_mut(&self) -> &mut RegisterBlock {
34 #[cfg(can_fdcan_h7)]
35 let ptr = self.msgram.ram(self.msg_ram_offset / 4).as_ptr() as *mut RegisterBlock;
36
37 #[cfg(not(can_fdcan_h7))]
38 let ptr = self.msgram.as_ptr() as *mut RegisterBlock;
39
40 unsafe { &mut (*ptr) }
41 }
42
43 fn rx_fifo_element(&self, fifonr: usize, bufnum: usize) -> &mut RxFifoElement {
44 &mut self.msg_ram_mut().receive[fifonr].fxsa[bufnum]
45 }
46
47 pub fn read<F: CanHeader>(&self, fifonr: usize) -> Option<(F, u16)> {
48 // Fill level - do we have a msg?
49 if self.regs.rxfs(fifonr).read().ffl() < 1 {
50 return None;
51 }
52
53 let read_idx = self.regs.rxfs(fifonr).read().fgi();
54 let mailbox = self.rx_fifo_element(fifonr, read_idx as usize);
55
56 let mut buffer = [0u8; 64];
57 let maybe_header = extract_frame(mailbox, &mut buffer);
58
59 // Clear FIFO, reduces count and increments read buf
60 self.regs.rxfa(fifonr).modify(|w| w.set_fai(read_idx));
61
62 match maybe_header {
63 Some((header, ts)) => {
64 let data = &buffer[0..header.len() as usize];
65 match F::from_header(header, data) {
66 Ok(frame) => Some((frame, ts)),
67 Err(_) => None,
68 }
69 }
70 None => None,
71 }
72 }
73
74 pub fn put_tx_frame(&self, bufidx: usize, header: &Header, buffer: &[u8]) {
75 let mailbox = self.tx_buffer_element(bufidx);
76 mailbox.reset();
77 put_tx_header(mailbox, header);
78 put_tx_data(mailbox, &buffer[..header.len() as usize]);
79
80 // Set <idx as Mailbox> as ready to transmit
81 self.regs.txbar().modify(|w| w.set_ar(bufidx, true));
82 }
83
84 fn reg_to_error(value: u8) -> Option<BusError> {
85 match value {
86 //0b000 => None,
87 0b001 => Some(BusError::Stuff),
88 0b010 => Some(BusError::Form),
89 0b011 => Some(BusError::Acknowledge),
90 0b100 => Some(BusError::BitRecessive),
91 0b101 => Some(BusError::BitDominant),
92 0b110 => Some(BusError::Crc),
93 //0b111 => Some(BusError::NoError),
94 _ => None,
95 }
96 }
97
98 pub fn curr_error(&self) -> Option<BusError> {
99 let err = { self.regs.psr().read() };
100 if err.bo() {
101 return Some(BusError::BusOff);
102 } else if err.ep() {
103 return Some(BusError::BusPassive);
104 } else if err.ew() {
105 return Some(BusError::BusWarning);
106 } else {
107 cfg_if! {
108 if #[cfg(can_fdcan_h7)] {
109 let lec = err.lec();
110 } else {
111 let lec = err.lec().to_bits();
112 }
113 }
114 if let Some(err) = Self::reg_to_error(lec) {
115 return Some(err);
116 }
117 }
118 None
119 }
120 /// Returns if the tx queue is able to accept new messages without having to cancel an existing one
121 #[inline]
122 pub fn tx_queue_is_full(&self) -> bool {
123 self.regs.txfqs().read().tfqf()
124 }
125
126 /// Returns the current TX buffer operation mode (queue or FIFO)
127 #[inline]
128 pub fn tx_queue_mode(&self) -> TxBufferMode {
129 self.regs.txbc().read().tfqm().into()
130 }
131
132 #[inline]
133 pub fn has_pending_frame(&self, idx: usize) -> bool {
134 self.regs.txbrp().read().trp(idx)
135 }
136
137 /// Returns `Ok` when the mailbox is free or if it contains pending frame with a
138 /// lower priority (higher ID) than the identifier `id`.
139 #[inline]
140 pub fn is_available(&self, bufidx: usize, id: &embedded_can::Id) -> bool {
141 if self.has_pending_frame(bufidx) {
142 let mailbox = self.tx_buffer_element(bufidx);
143
144 let header_reg = mailbox.header.read();
145 let old_id = make_id(header_reg.id().bits(), header_reg.xtd().bits());
146
147 *id > old_id
148 } else {
149 true
150 }
151 }
152
153 /// Attempts to abort the sending of a frame that is pending in a mailbox.
154 ///
155 /// If there is no frame in the provided mailbox, or its transmission succeeds before it can be
156 /// aborted, this function has no effect and returns `false`.
157 ///
158 /// If there is a frame in the provided mailbox, and it is canceled successfully, this function
159 /// returns `true`.
160 #[inline]
161 pub fn abort(&self, bufidx: usize) -> bool {
162 let can = self.regs;
163
164 // Check if there is a request pending to abort
165 if self.has_pending_frame(bufidx) {
166 // Abort Request
167 can.txbcr().write(|w| w.set_cr(bufidx, true));
168
169 // Wait for the abort request to be finished.
170 loop {
171 if can.txbcf().read().cf(bufidx) {
172 // Return false when a transmission has occured
173 break can.txbto().read().to(bufidx) == false;
174 }
175 }
176 } else {
177 false
178 }
179 }
180
181 #[inline]
182 fn abort_pending_mailbox<F: embedded_can::Frame>(&self, bufidx: usize) -> Option<F> {
183 if self.abort(bufidx) {
184 let mailbox = self.tx_buffer_element(bufidx);
185
186 let header_reg = mailbox.header.read();
187 let id = make_id(header_reg.id().bits(), header_reg.xtd().bits());
188
189 let len = match header_reg.to_data_length() {
190 DataLength::Fdcan(len) => len,
191 DataLength::Classic(len) => len,
192 };
193 if len as usize > ClassicData::MAX_DATA_LEN {
194 return None;
195 }
196
197 let mut data = [0u8; 64];
198 data_from_tx_buffer(&mut data, mailbox, len as usize);
199
200 if header_reg.rtr().bit() {
201 F::new_remote(id, len as usize)
202 } else {
203 F::new(id, &data[0..(len as usize)])
204 }
205 } else {
206 // Abort request failed because the frame was already sent (or being sent) on
207 // the bus. All mailboxes are now free. This can happen for small prescaler
208 // values (e.g. 1MBit/s bit timing with a source clock of 8MHz) or when an ISR
209 // has preempted the execution.
210 None
211 }
212 }
213
214 pub fn write<F: embedded_can::Frame + CanHeader>(&self, frame: &F) -> nb::Result<Option<F>, Infallible> {
215 let (idx, pending_frame) = if self.tx_queue_is_full() {
216 if self.tx_queue_mode() == TxBufferMode::Fifo {
217 // Does not make sense to cancel a pending frame when using FIFO
218 return Err(nb::Error::WouldBlock);
219 }
220 // If the queue is full,
221 // Discard the first slot with a lower priority message
222 let id = frame.header().id();
223 if self.is_available(0, id) {
224 (0, self.abort_pending_mailbox(0))
225 } else if self.is_available(1, id) {
226 (1, self.abort_pending_mailbox(1))
227 } else if self.is_available(2, id) {
228 (2, self.abort_pending_mailbox(2))
229 } else {
230 // For now we bail when there is no lower priority slot available
231 // Can this lead to priority inversion?
232 return Err(nb::Error::WouldBlock);
233 }
234 } else {
235 // Read the Write Pointer
236 let idx = self.regs.txfqs().read().tfqpi();
237
238 (idx, None)
239 };
240
241 self.put_tx_frame(idx as usize, frame.header(), frame.data());
242
243 Ok(pending_frame)
244 }
245
246 #[inline]
247 fn reset_msg_ram(&self) {
248 self.msg_ram_mut().reset();
249 }
250
251 #[inline]
252 fn enter_init_mode(&self) {
253 self.regs.cccr().modify(|w| w.set_init(true));
254 while false == self.regs.cccr().read().init() {}
255 self.regs.cccr().modify(|w| w.set_cce(true));
256 }
257
258 /// Enables or disables loopback mode: Internally connects the TX and RX
259 /// signals together.
260 #[inline]
261 fn set_loopback_mode(&self, mode: LoopbackMode) {
262 let (test, mon, lbck) = match mode {
263 LoopbackMode::None => (false, false, false),
264 LoopbackMode::Internal => (true, true, true),
265 LoopbackMode::External => (true, false, true),
266 };
267
268 self.set_test_mode(test);
269 self.set_bus_monitoring_mode(mon);
270
271 self.regs.test().modify(|w| w.set_lbck(lbck));
272 }
273
274 /// Enables or disables silent mode: Disconnects the TX signal from the pin.
275 #[inline]
276 fn set_bus_monitoring_mode(&self, enabled: bool) {
277 self.regs.cccr().modify(|w| w.set_mon(enabled));
278 }
279
280 #[inline]
281 fn set_restricted_operations(&self, enabled: bool) {
282 self.regs.cccr().modify(|w| w.set_asm(enabled));
283 }
284
285 #[inline]
286 fn set_normal_operations(&self, _enabled: bool) {
287 self.set_loopback_mode(LoopbackMode::None);
288 }
289
290 #[inline]
291 fn set_test_mode(&self, enabled: bool) {
292 self.regs.cccr().modify(|w| w.set_test(enabled));
293 }
294
295 #[inline]
296 fn set_power_down_mode(&self, enabled: bool) {
297 self.regs.cccr().modify(|w| w.set_csr(enabled));
298 while self.regs.cccr().read().csa() != enabled {}
299 }
300
301 /// Moves out of PoweredDownMode and into ConfigMode
302 #[inline]
303 pub fn into_config_mode(self, _config: FdCanConfig) {
304 self.set_power_down_mode(false);
305 self.enter_init_mode();
306 self.reset_msg_ram();
307
308 // check the FDCAN core matches our expections
309 assert!(
310 self.regs.crel().read().rel() == 3,
311 "Expected FDCAN core major release 3"
312 );
313 assert!(
314 self.regs.endn().read().etv() == 0x87654321_u32,
315 "Error reading endianness test value from FDCAN core"
316 );
317
318 /*
319 for fid in 0..crate::can::message_ram::STANDARD_FILTER_MAX {
320 self.set_standard_filter((fid as u8).into(), StandardFilter::disable());
321 }
322 for fid in 0..Ecrate::can::message_ram::XTENDED_FILTER_MAX {
323 self.set_extended_filter(fid.into(), ExtendedFilter::disable());
324 }
325 */
326 }
327
328 /// Applies the settings of a new FdCanConfig See [`FdCanConfig`]
329 #[inline]
330 pub fn apply_config(&self, config: FdCanConfig) {
331 self.set_tx_buffer_mode(config.tx_buffer_mode);
332
333 // set standard filters list size to 28
334 // set extended filters list size to 8
335 // REQUIRED: we use the memory map as if these settings are set
336 // instead of re-calculating them.
337 #[cfg(not(can_fdcan_h7))]
338 {
339 self.regs.rxgfc().modify(|w| {
340 w.set_lss(crate::can::fd::message_ram::STANDARD_FILTER_MAX);
341 w.set_lse(crate::can::fd::message_ram::EXTENDED_FILTER_MAX);
342 });
343 }
344 #[cfg(can_fdcan_h7)]
345 {
346 self.regs
347 .sidfc()
348 .modify(|w| w.set_lss(crate::can::fd::message_ram::STANDARD_FILTER_MAX));
349 self.regs
350 .xidfc()
351 .modify(|w| w.set_lse(crate::can::fd::message_ram::EXTENDED_FILTER_MAX));
352 }
353
354 self.configure_msg_ram();
355
356 // Enable timestamping
357 #[cfg(not(can_fdcan_h7))]
358 self.regs
359 .tscc()
360 .write(|w| w.set_tss(stm32_metapac::can::vals::Tss::INCREMENT));
361 #[cfg(can_fdcan_h7)]
362 self.regs.tscc().write(|w| w.set_tss(0x01));
363
364 // this isn't really documented in the reference manual
365 // but corresponding txbtie bit has to be set for the TC (TxComplete) interrupt to fire
366 self.regs.txbtie().write(|w| w.0 = 0xffff_ffff);
367 self.regs.ie().modify(|w| {
368 w.set_rfne(0, true); // Rx Fifo 0 New Msg
369 w.set_rfne(1, true); // Rx Fifo 1 New Msg
370 w.set_tce(true); // Tx Complete
371 w.set_boe(true); // Bus-Off Status Changed
372 });
373 self.regs.ile().modify(|w| {
374 w.set_eint0(true); // Interrupt Line 0
375 w.set_eint1(true); // Interrupt Line 1
376 });
377
378 self.set_data_bit_timing(config.dbtr);
379 self.set_nominal_bit_timing(config.nbtr);
380 self.set_automatic_retransmit(config.automatic_retransmit);
381 self.set_transmit_pause(config.transmit_pause);
382 self.set_frame_transmit(config.frame_transmit);
383 //self.set_interrupt_line_config(config.interrupt_line_config);
384 self.set_non_iso_mode(config.non_iso_mode);
385 self.set_edge_filtering(config.edge_filtering);
386 self.set_protocol_exception_handling(config.protocol_exception_handling);
387 self.set_global_filter(config.global_filter);
388 }
389
390 #[inline]
391 fn leave_init_mode(&self, config: FdCanConfig) {
392 self.apply_config(config);
393
394 self.regs.cccr().modify(|w| w.set_cce(false));
395 self.regs.cccr().modify(|w| w.set_init(false));
396 while self.regs.cccr().read().init() == true {}
397 }
398
399 /// Moves out of ConfigMode and into specified mode
400 #[inline]
401 pub fn into_mode(&self, config: FdCanConfig, mode: crate::can::_version::OperatingMode) {
402 match mode {
403 crate::can::OperatingMode::InternalLoopbackMode => self.set_loopback_mode(LoopbackMode::Internal),
404 crate::can::OperatingMode::ExternalLoopbackMode => self.set_loopback_mode(LoopbackMode::External),
405 crate::can::OperatingMode::NormalOperationMode => self.set_normal_operations(true),
406 crate::can::OperatingMode::RestrictedOperationMode => self.set_restricted_operations(true),
407 crate::can::OperatingMode::BusMonitoringMode => self.set_bus_monitoring_mode(true),
408 }
409 self.leave_init_mode(config);
410 }
411
412 /// Configures the bit timings.
413 ///
414 /// You can use <http://www.bittiming.can-wiki.info/> to calculate the `btr` parameter. Enter
415 /// parameters as follows:
416 ///
417 /// - *Clock Rate*: The input clock speed to the CAN peripheral (*not* the CPU clock speed).
418 /// This is the clock rate of the peripheral bus the CAN peripheral is attached to (eg. APB1).
419 /// - *Sample Point*: Should normally be left at the default value of 87.5%.
420 /// - *SJW*: Should normally be left at the default value of 1.
421 ///
422 /// Then copy the `CAN_BUS_TIME` register value from the table and pass it as the `btr`
423 /// parameter to this method.
424 #[inline]
425 pub fn set_nominal_bit_timing(&self, btr: NominalBitTiming) {
426 self.regs.nbtp().write(|w| {
427 w.set_nbrp(btr.nbrp() - 1);
428 w.set_ntseg1(btr.ntseg1() - 1);
429 w.set_ntseg2(btr.ntseg2() - 1);
430 w.set_nsjw(btr.nsjw() - 1);
431 });
432 }
433
434 /// Configures the data bit timings for the FdCan Variable Bitrates.
435 /// This is not used when frame_transmit is set to anything other than AllowFdCanAndBRS.
436 #[inline]
437 pub fn set_data_bit_timing(&self, btr: DataBitTiming) {
438 self.regs.dbtp().write(|w| {
439 w.set_dbrp(btr.dbrp() - 1);
440 w.set_dtseg1(btr.dtseg1() - 1);
441 w.set_dtseg2(btr.dtseg2() - 1);
442 w.set_dsjw(btr.dsjw() - 1);
443 });
444 }
445
446 /// Enables or disables automatic retransmission of messages
447 ///
448 /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame
449 /// util it can be sent. Otherwise, it will try only once to send each frame.
450 ///
451 /// Automatic retransmission is enabled by default.
452 #[inline]
453 pub fn set_automatic_retransmit(&self, enabled: bool) {
454 self.regs.cccr().modify(|w| w.set_dar(!enabled));
455 }
456
457 /// Configures the transmit pause feature. See
458 /// [`FdCanConfig::set_transmit_pause`]
459 #[inline]
460 pub fn set_transmit_pause(&self, enabled: bool) {
461 self.regs.cccr().modify(|w| w.set_txp(!enabled));
462 }
463
464 /// Configures non-iso mode. See [`FdCanConfig::set_non_iso_mode`]
465 #[inline]
466 pub fn set_non_iso_mode(&self, enabled: bool) {
467 self.regs.cccr().modify(|w| w.set_niso(enabled));
468 }
469
470 /// Configures edge filtering. See [`FdCanConfig::set_edge_filtering`]
471 #[inline]
472 pub fn set_edge_filtering(&self, enabled: bool) {
473 self.regs.cccr().modify(|w| w.set_efbi(enabled));
474 }
475
476 /// Configures TX Buffer Mode
477 #[inline]
478 pub fn set_tx_buffer_mode(&self, tbm: TxBufferMode) {
479 self.regs.txbc().write(|w| w.set_tfqm(tbm.into()));
480 }
481
482 /// Configures frame transmission mode. See
483 /// [`FdCanConfig::set_frame_transmit`]
484 #[inline]
485 pub fn set_frame_transmit(&self, fts: FrameTransmissionConfig) {
486 let (fdoe, brse) = match fts {
487 FrameTransmissionConfig::ClassicCanOnly => (false, false),
488 FrameTransmissionConfig::AllowFdCan => (true, false),
489 FrameTransmissionConfig::AllowFdCanAndBRS => (true, true),
490 };
491
492 self.regs.cccr().modify(|w| {
493 w.set_fdoe(fdoe);
494 #[cfg(can_fdcan_h7)]
495 w.set_bse(brse);
496 #[cfg(not(can_fdcan_h7))]
497 w.set_brse(brse);
498 });
499 }
500
501 /// Sets the protocol exception handling on/off
502 #[inline]
503 pub fn set_protocol_exception_handling(&self, enabled: bool) {
504 self.regs.cccr().modify(|w| w.set_pxhd(!enabled));
505 }
506
507 /// Configures and resets the timestamp counter
508 #[inline]
509 #[allow(unused)]
510 pub fn set_timestamp_counter_source(&self, select: TimestampSource) {
511 #[cfg(can_fdcan_h7)]
512 let (tcp, tss) = match select {
513 TimestampSource::None => (0, 0),
514 TimestampSource::Prescaler(p) => (p as u8, 1),
515 TimestampSource::FromTIM3 => (0, 2),
516 };
517
518 #[cfg(not(can_fdcan_h7))]
519 let (tcp, tss) = match select {
520 TimestampSource::None => (0, stm32_metapac::can::vals::Tss::ZERO),
521 TimestampSource::Prescaler(p) => (p as u8, stm32_metapac::can::vals::Tss::INCREMENT),
522 TimestampSource::FromTIM3 => (0, stm32_metapac::can::vals::Tss::EXTERNAL),
523 };
524
525 self.regs.tscc().write(|w| {
526 w.set_tcp(tcp);
527 w.set_tss(tss);
528 });
529 }
530
531 #[cfg(not(can_fdcan_h7))]
532 /// Configures the global filter settings
533 #[inline]
534 pub fn set_global_filter(&self, filter: GlobalFilter) {
535 let anfs = match filter.handle_standard_frames {
536 crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => stm32_metapac::can::vals::Anfs::ACCEPT_FIFO_0,
537 crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => stm32_metapac::can::vals::Anfs::ACCEPT_FIFO_1,
538 crate::can::fd::config::NonMatchingFilter::Reject => stm32_metapac::can::vals::Anfs::REJECT,
539 };
540 let anfe = match filter.handle_extended_frames {
541 crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => stm32_metapac::can::vals::Anfe::ACCEPT_FIFO_0,
542 crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => stm32_metapac::can::vals::Anfe::ACCEPT_FIFO_1,
543 crate::can::fd::config::NonMatchingFilter::Reject => stm32_metapac::can::vals::Anfe::REJECT,
544 };
545
546 self.regs.rxgfc().modify(|w| {
547 w.set_anfs(anfs);
548 w.set_anfe(anfe);
549 w.set_rrfs(filter.reject_remote_standard_frames);
550 w.set_rrfe(filter.reject_remote_extended_frames);
551 });
552 }
553
554 #[cfg(can_fdcan_h7)]
555 /// Configures the global filter settings
556 #[inline]
557 pub fn set_global_filter(&self, filter: GlobalFilter) {
558 let anfs = match filter.handle_standard_frames {
559 crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => 0,
560 crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => 1,
561 crate::can::fd::config::NonMatchingFilter::Reject => 2,
562 };
563
564 let anfe = match filter.handle_extended_frames {
565 crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => 0,
566 crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => 1,
567 crate::can::fd::config::NonMatchingFilter::Reject => 2,
568 };
569
570 self.regs.gfc().modify(|w| {
571 w.set_anfs(anfs);
572 w.set_anfe(anfe);
573 w.set_rrfs(filter.reject_remote_standard_frames);
574 w.set_rrfe(filter.reject_remote_extended_frames);
575 });
576 }
577
578 #[cfg(not(can_fdcan_h7))]
579 fn configure_msg_ram(&self) {}
580
581 #[cfg(can_fdcan_h7)]
582 fn configure_msg_ram(&self) {
583 let r = self.regs;
584
585 use crate::can::fd::message_ram::*;
586 //use fdcan::message_ram::*;
587 let mut offset_words = (self.msg_ram_offset / 4) as u16;
588
589 // 11-bit filter
590 r.sidfc().modify(|w| w.set_flssa(offset_words));
591 offset_words += STANDARD_FILTER_MAX as u16;
592
593 // 29-bit filter
594 r.xidfc().modify(|w| w.set_flesa(offset_words));
595 offset_words += 2 * EXTENDED_FILTER_MAX as u16;
596
597 // Rx FIFO 0 and 1
598 for i in 0..=1 {
599 r.rxfc(i).modify(|w| {
600 w.set_fsa(offset_words);
601 w.set_fs(RX_FIFO_MAX);
602 w.set_fwm(RX_FIFO_MAX);
603 });
604 offset_words += 18 * RX_FIFO_MAX as u16;
605 }
606
607 // Rx buffer - see below
608 // Tx event FIFO
609 r.txefc().modify(|w| {
610 w.set_efsa(offset_words);
611 w.set_efs(TX_EVENT_MAX);
612 w.set_efwm(TX_EVENT_MAX);
613 });
614 offset_words += 2 * TX_EVENT_MAX as u16;
615
616 // Tx buffers
617 r.txbc().modify(|w| {
618 w.set_tbsa(offset_words);
619 w.set_tfqs(TX_FIFO_MAX);
620 });
621 offset_words += 18 * TX_FIFO_MAX as u16;
622
623 // Rx Buffer - not used
624 r.rxbc().modify(|w| {
625 w.set_rbsa(offset_words);
626 });
627
628 // TX event FIFO?
629 // Trigger memory?
630
631 // Set the element sizes to 16 bytes
632 r.rxesc().modify(|w| {
633 w.set_rbds(0b111);
634 for i in 0..=1 {
635 w.set_fds(i, 0b111);
636 }
637 });
638 r.txesc().modify(|w| {
639 w.set_tbds(0b111);
640 })
641 }
642}
643
644fn make_id(id: u32, extended: bool) -> embedded_can::Id {
645 if extended {
646 embedded_can::Id::from(unsafe { embedded_can::ExtendedId::new_unchecked(raw:id & 0x1FFFFFFF) })
647 } else {
648 // A standard identifier is stored into ID[28:18].
649 embedded_can::Id::from(unsafe { embedded_can::StandardId::new_unchecked(((id >> 18) & 0x000007FF) as u16) })
650 }
651}
652
653fn put_tx_header(mailbox: &mut TxBufferElement, header: &Header) {
654 let (id, id_type) = match header.id() {
655 // A standard identifier has to be written to ID[28:18].
656 embedded_can::Id::Standard(id) => ((id.as_raw() as u32) << 18, IdType::StandardId),
657 embedded_can::Id::Extended(id) => (id.as_raw() as u32, IdType::ExtendedId),
658 };
659
660 // Use FDCAN only for DLC > 8. FDCAN users can revise this if required.
661 let frame_format = if header.len() > 8 || header.fdcan() {
662 FrameFormat::Fdcan
663 } else {
664 FrameFormat::Classic
665 };
666 let brs = (frame_format == FrameFormat::Fdcan) && header.bit_rate_switching();
667
668 mailbox.header.write(|w| {
669 unsafe { w.id().bits(id) }
670 .rtr()
671 .bit(header.len() == 0 && header.rtr())
672 .xtd()
673 .set_id_type(id_type)
674 .set_len(DataLength::new(header.len(), frame_format))
675 .set_event(Event::NoEvent)
676 .fdf()
677 .set_format(frame_format)
678 .brs()
679 .bit(brs)
680 //esi.set_error_indicator(//TODO//)
681 });
682}
683
684fn put_tx_data(mailbox: &mut TxBufferElement, buffer: &[u8]) {
685 let mut lbuffer: [u32; 16] = [0_u32; 16];
686 let len: usize = buffer.len();
687 let data: &mut [u8] = unsafe { slice::from_raw_parts_mut(data:lbuffer.as_mut_ptr() as *mut u8, len) };
688 data[..len].copy_from_slice(&buffer[..len]);
689 let data_len: usize = ((len) + 3) / 4;
690 for (register: &mut RW, byte: &u32) in mailbox.data.iter_mut().zip(lbuffer[..data_len].iter()) {
691 unsafe { register.write(*byte) };
692 }
693}
694
695fn data_from_fifo(buffer: &mut [u8], mailbox: &RxFifoElement, len: usize) {
696 for (i: usize, register: &RW) in mailbox.data.iter().enumerate() {
697 let register_value: u32 = register.read();
698 let register_bytes: &[u8] = unsafe { slice::from_raw_parts(&register_value as *const u32 as *const u8, len:4) };
699 let num_bytes: usize = (len) - i * 4;
700 if num_bytes <= 4 {
701 buffer[i * 4..i * 4 + num_bytes].copy_from_slice(&register_bytes[..num_bytes]);
702 break;
703 }
704 buffer[i * 4..(i + 1) * 4].copy_from_slice(src:register_bytes);
705 }
706}
707
708fn data_from_tx_buffer(buffer: &mut [u8], mailbox: &TxBufferElement, len: usize) {
709 for (i: usize, register: &RW) in mailbox.data.iter().enumerate() {
710 let register_value: u32 = register.read();
711 let register_bytes: &[u8] = unsafe { slice::from_raw_parts(&register_value as *const u32 as *const u8, len:4) };
712 let num_bytes: usize = (len) - i * 4;
713 if num_bytes <= 4 {
714 buffer[i * 4..i * 4 + num_bytes].copy_from_slice(&register_bytes[..num_bytes]);
715 break;
716 }
717 buffer[i * 4..(i + 1) * 4].copy_from_slice(src:register_bytes);
718 }
719}
720
721fn extract_frame(mailbox: &RxFifoElement, buffer: &mut [u8]) -> Option<(Header, u16)> {
722 let header_reg: R<[u32; 2], Reg<[u32; 2], …>> = mailbox.header.read();
723
724 let id: Id = make_id(id:header_reg.id().bits(), extended:header_reg.xtd().bits());
725 let dlc: u8 = header_reg.to_data_length().len();
726 let len: usize = dlc as usize;
727 let timestamp: u16 = header_reg.txts().bits;
728 if len > buffer.len() {
729 return None;
730 }
731 data_from_fifo(buffer, mailbox, len);
732 let header: Header = if header_reg.fdf().bits {
733 Header::new_fd(id, len:dlc, rtr:header_reg.rtr().bits(), brs:header_reg.brs().bits())
734 } else {
735 Header::new(id, len:dlc, rtr:header_reg.rtr().bits())
736 };
737 Some((header, timestamp))
738}
739