From 0ed402fd79a6dfbe4f3b2187a97d160ce7c3140b Mon Sep 17 00:00:00 2001 From: Torin Cooper-Bennun Date: Tue, 27 Feb 2024 23:42:50 +0000 Subject: [PATCH 1/2] stm32: can: fd: refactor out some duplicate code --- embassy-stm32/src/can/fd/peripheral.rs | 122 +++---------------------- embassy-stm32/src/can/fdcan.rs | 66 ++++++------- embassy-stm32/src/can/frame.rs | 30 ++++++ 3 files changed, 68 insertions(+), 150 deletions(-) diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs index 9c29e4887..7d26a5fe0 100644 --- a/embassy-stm32/src/can/fd/peripheral.rs +++ b/embassy-stm32/src/can/fd/peripheral.rs @@ -37,7 +37,7 @@ impl Registers { &mut self.msg_ram_mut().receive[fifonr].fxsa[bufnum] } - pub fn read_classic(&self, fifonr: usize) -> Option<(ClassicFrame, u16)> { + pub fn read(&self, fifonr: usize) -> Option<(F, u16)> { // Fill level - do we have a msg? if self.regs.rxfs(fifonr).read().ffl() < 1 { return None; @@ -54,32 +54,8 @@ impl Registers { match maybe_header { Some((header, ts)) => { - let data = ClassicData::new(&buffer[0..header.len() as usize]); - Some((ClassicFrame::new(header, data.unwrap()), ts)) - } - None => None, - } - } - - pub fn read_fd(&self, fifonr: usize) -> Option<(FdFrame, u16)> { - // Fill level - do we have a msg? - if self.regs.rxfs(fifonr).read().ffl() < 1 { - return None; - } - - let read_idx = self.regs.rxfs(fifonr).read().fgi(); - let mailbox = self.rx_fifo_element(fifonr, read_idx as usize); - - let mut buffer: [u8; 64] = [0; 64]; - let maybe_header = extract_frame(mailbox, &mut buffer); - - // Clear FIFO, reduces count and increments read buf - self.regs.rxfa(fifonr).modify(|w| w.set_fai(read_idx)); - - match maybe_header { - Some((header, ts)) => { - let data = FdData::new(&buffer[0..header.len() as usize]); - Some((FdFrame::new(header, data.unwrap()), ts)) + let data = &buffer[0..header.len() as usize]; + Some((F::from_header(header, data)?, ts)) } None => None, } @@ -194,10 +170,9 @@ impl Registers { #[inline] //fn abort_pending_mailbox(&mut self, idx: Mailbox, pending: PTX) -> Option - pub fn abort_pending_mailbox(&self, bufidx: usize) -> Option //where // PTX: FnOnce(Mailbox, TxFrameHeader, &[u32]) -> R, - { + pub fn abort_pending_mailbox_generic(&self, bufidx: usize) -> Option { if self.abort(bufidx) { let mailbox = self.tx_buffer_element(bufidx); @@ -216,50 +191,11 @@ impl Registers { let mut data = [0u8; 64]; data_from_tx_buffer(&mut data, mailbox, len as usize); - let cd = ClassicData::new(&data).unwrap(); - Some(ClassicFrame::new(Header::new(id, len, header_reg.rtr().bit()), cd)) - } else { - // Abort request failed because the frame was already sent (or being sent) on - // the bus. All mailboxes are now free. This can happen for small prescaler - // values (e.g. 1MBit/s bit timing with a source clock of 8MHz) or when an ISR - // has preempted the execution. - None - } - } - - #[inline] - //fn abort_pending_mailbox(&mut self, idx: Mailbox, pending: PTX) -> Option - pub fn abort_pending_fd_mailbox(&self, bufidx: usize) -> Option -//where - // PTX: FnOnce(Mailbox, TxFrameHeader, &[u32]) -> R, - { - if self.abort(bufidx) { - let mailbox = self.tx_buffer_element(bufidx); - - let header_reg = mailbox.header.read(); - let id = make_id(header_reg.id().bits(), header_reg.xtd().bits()); - - let len = match header_reg.to_data_length() { - DataLength::Fdcan(len) => len, - DataLength::Classic(len) => len, - }; - if len as usize > FdFrame::MAX_DATA_LEN { - return None; - } - - //let tx_ram = self.tx_msg_ram(); - let mut data = [0u8; 64]; - data_from_tx_buffer(&mut data, mailbox, len as usize); - - let cd = FdData::new(&data).unwrap(); - - let header = if header_reg.fdf().frame_format() == FrameFormat::Fdcan { - Header::new_fd(id, len, header_reg.rtr().bit(), header_reg.brs().bit()) + if header_reg.rtr().bit() { + F::new_remote(id, len as usize) } else { - Header::new(id, len, header_reg.rtr().bit()) - }; - - Some(FdFrame::new(header, cd)) + F::new(id, &data) + } } else { // Abort request failed because the frame was already sent (or being sent) on // the bus. All mailboxes are now free. This can happen for small prescaler @@ -272,7 +208,7 @@ impl Registers { /// As Transmit, but if there is a pending frame, `pending` will be called so that the frame can /// be preserved. //pub fn transmit_preserve( - pub fn write_classic(&self, frame: &ClassicFrame) -> nb::Result, Infallible> { + pub fn write(&self, frame: &F) -> nb::Result, Infallible> { let queue_is_full = self.tx_queue_is_full(); let id = frame.header().id(); @@ -281,45 +217,11 @@ impl Registers { // Discard the first slot with a lower priority message let (idx, pending_frame) = if queue_is_full { if self.is_available(0, id) { - (0, self.abort_pending_mailbox(0)) + (0, self.abort_pending_mailbox_generic(0)) } else if self.is_available(1, id) { - (1, self.abort_pending_mailbox(1)) + (1, self.abort_pending_mailbox_generic(1)) } else if self.is_available(2, id) { - (2, self.abort_pending_mailbox(2)) - } else { - // For now we bail when there is no lower priority slot available - // Can this lead to priority inversion? - return Err(nb::Error::WouldBlock); - } - } else { - // Read the Write Pointer - let idx = self.regs.txfqs().read().tfqpi(); - - (idx, None) - }; - - self.put_tx_frame(idx as usize, frame.header(), frame.data()); - - Ok(pending_frame) - } - - /// As Transmit, but if there is a pending frame, `pending` will be called so that the frame can - /// be preserved. - //pub fn transmit_preserve( - pub fn write_fd(&self, frame: &FdFrame) -> nb::Result, Infallible> { - let queue_is_full = self.tx_queue_is_full(); - - let id = frame.header().id(); - - // If the queue is full, - // Discard the first slot with a lower priority message - let (idx, pending_frame) = if queue_is_full { - if self.is_available(0, id) { - (0, self.abort_pending_fd_mailbox(0)) - } else if self.is_available(1, id) { - (1, self.abort_pending_fd_mailbox(1)) - } else if self.is_available(2, id) { - (2, self.abort_pending_fd_mailbox(2)) + (2, self.abort_pending_mailbox_generic(2)) } else { // For now we bail when there is no lower priority slot available // Can this lead to priority inversion? diff --git a/embassy-stm32/src/can/fdcan.rs b/embassy-stm32/src/can/fdcan.rs index 744d756f5..6a4a25cb7 100644 --- a/embassy-stm32/src/can/fdcan.rs +++ b/embassy-stm32/src/can/fdcan.rs @@ -58,7 +58,7 @@ impl interrupt::typelevel::Handler for IT0Interrup if !T::registers().tx_queue_is_full() { match buf.tx_receiver.try_receive() { Ok(frame) => { - _ = T::registers().write_classic(&frame); + _ = T::registers().write(&frame); } Err(_) => {} } @@ -68,7 +68,7 @@ impl interrupt::typelevel::Handler for IT0Interrup if !T::registers().tx_queue_is_full() { match buf.tx_receiver.try_receive() { Ok(frame) => { - _ = T::registers().write_fd(&frame); + _ = T::registers().write(&frame); } Err(_) => {} } @@ -359,7 +359,7 @@ impl<'d, T: Instance> Fdcan<'d, T> { /// Returns the next received message frame pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> { - T::state().rx_mode.read::().await + T::state().rx_mode.read_classic::().await } /// Queues the message to be sent but exerts backpressure. If a lower-priority @@ -633,7 +633,7 @@ impl<'c, 'd, T: Instance> FdcanTx<'d, T> { impl<'c, 'd, T: Instance> FdcanRx<'d, T> { /// Returns the next received message frame pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> { - T::state().rx_mode.read::().await + T::state().rx_mode.read_classic::().await } /// Returns the next received message frame @@ -649,6 +649,7 @@ pub(crate) mod sealed { use embassy_sync::channel::{DynamicReceiver, DynamicSender}; use embassy_sync::waitqueue::AtomicWaker; + use super::CanHeader; use crate::can::_version::{BusError, Timestamp}; use crate::can::frame::{ClassicFrame, FdFrame}; @@ -689,13 +690,13 @@ pub(crate) mod sealed { waker.wake(); } RxMode::ClassicBuffered(buf) => { - if let Some(r) = T::registers().read_classic(fifonr) { + if let Some(r) = T::registers().read(fifonr) { let ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1); let _ = buf.rx_sender.try_send((r.0, ts)); } } RxMode::FdBuffered(buf) => { - if let Some(r) = T::registers().read_fd(fifonr) { + if let Some(r) = T::registers().read(fifonr) { let ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1); let _ = buf.rx_sender.try_send((r.0, ts)); } @@ -703,15 +704,15 @@ pub(crate) mod sealed { } } - pub async fn read(&self) -> Result<(ClassicFrame, Timestamp), BusError> { + async fn read(&self) -> Result<(F, Timestamp), BusError> { poll_fn(|cx| { T::state().err_waker.register(cx.waker()); self.register(cx.waker()); - if let Some((msg, ts)) = T::registers().read_classic(0) { + if let Some((msg, ts)) = T::registers().read(0) { let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts); return Poll::Ready(Ok((msg, ts))); - } else if let Some((msg, ts)) = T::registers().read_classic(1) { + } else if let Some((msg, ts)) = T::registers().read(1) { let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts); return Poll::Ready(Ok((msg, ts))); } else if let Some(err) = T::registers().curr_error() { @@ -723,24 +724,12 @@ pub(crate) mod sealed { .await } - pub async fn read_fd(&self) -> Result<(FdFrame, Timestamp), BusError> { - poll_fn(|cx| { - T::state().err_waker.register(cx.waker()); - self.register(cx.waker()); + pub async fn read_classic(&self) -> Result<(ClassicFrame, Timestamp), BusError> { + self.read::().await + } - if let Some((msg, ts)) = T::registers().read_fd(0) { - let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts); - return Poll::Ready(Ok((msg, ts))); - } else if let Some((msg, ts)) = T::registers().read_fd(1) { - let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts); - return Poll::Ready(Ok((msg, ts))); - } else if let Some(err) = T::registers().curr_error() { - // TODO: this is probably wrong - return Poll::Ready(Err(err)); - } - Poll::Pending - }) - .await + pub async fn read_fd(&self) -> Result<(FdFrame, Timestamp), BusError> { + self.read::().await } } @@ -766,11 +755,11 @@ pub(crate) mod sealed { /// frame is dropped from the mailbox, it is returned. If no lower-priority frames /// can be replaced, this call asynchronously waits for a frame to be successfully /// transmitted, then tries again. - pub async fn write(&self, frame: &ClassicFrame) -> Option { + async fn write_generic(&self, frame: &F) -> Option { poll_fn(|cx| { self.register(cx.waker()); - if let Ok(dropped) = T::registers().write_classic(frame) { + if let Ok(dropped) = T::registers().write(frame) { return Poll::Ready(dropped); } @@ -781,23 +770,20 @@ pub(crate) mod sealed { .await } + /// Queues the message to be sent but exerts backpressure. If a lower-priority + /// frame is dropped from the mailbox, it is returned. If no lower-priority frames + /// can be replaced, this call asynchronously waits for a frame to be successfully + /// transmitted, then tries again. + pub async fn write(&self, frame: &ClassicFrame) -> Option { + self.write_generic::(frame).await + } + /// Queues the message to be sent but exerts backpressure. If a lower-priority /// frame is dropped from the mailbox, it is returned. If no lower-priority frames /// can be replaced, this call asynchronously waits for a frame to be successfully /// transmitted, then tries again. pub async fn write_fd(&self, frame: &FdFrame) -> Option { - poll_fn(|cx| { - self.register(cx.waker()); - - if let Ok(dropped) = T::registers().write_fd(frame) { - return Poll::Ready(dropped); - } - - // Couldn't replace any lower priority frames. Need to wait for some mailboxes - // to clear. - Poll::Pending - }) - .await + self.write_generic::(frame).await } } diff --git a/embassy-stm32/src/can/frame.rs b/embassy-stm32/src/can/frame.rs index 725a9b1ab..59b9fb08c 100644 --- a/embassy-stm32/src/can/frame.rs +++ b/embassy-stm32/src/can/frame.rs @@ -56,6 +56,16 @@ impl Header { } } +/// Trait for FDCAN frame types, providing ability to construct from a Header +/// and to retrieve the Header from a frame +pub trait CanHeader: Sized { + /// Construct frame from header and payload + fn from_header(header: Header, data: &[u8]) -> Option; + + /// Get this frame's header struct + fn header(&self) -> &Header; +} + /// Payload of a classic CAN data frame. /// /// Contains 0 to 8 Bytes of data. @@ -213,6 +223,16 @@ impl embedded_can::Frame for ClassicFrame { } } +impl CanHeader for ClassicFrame { + fn from_header(header: Header, data: &[u8]) -> Option { + Some(Self::new(header, ClassicData::new(data)?)) + } + + fn header(&self) -> &Header { + self.header() + } +} + /// Payload of a (FD)CAN data frame. /// /// Contains 0 to 64 Bytes of data. @@ -368,3 +388,13 @@ impl embedded_can::Frame for FdFrame { &self.data.raw() } } + +impl CanHeader for FdFrame { + fn from_header(header: Header, data: &[u8]) -> Option { + Some(Self::new(header, FdData::new(data)?)) + } + + fn header(&self) -> &Header { + self.header() + } +} From a8da42943fdd57ded612399d18846398cbd011d3 Mon Sep 17 00:00:00 2001 From: Torin Cooper-Bennun Date: Tue, 27 Feb 2024 23:45:53 +0000 Subject: [PATCH 2/2] stm32: can: fd: rm some irrelevant commented code and dead code --- embassy-stm32/src/can/fd/peripheral.rs | 42 -------------------------- embassy-stm32/src/can/frame.rs | 2 -- 2 files changed, 44 deletions(-) diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs index 7d26a5fe0..c5606b883 100644 --- a/embassy-stm32/src/can/fd/peripheral.rs +++ b/embassy-stm32/src/can/fd/peripheral.rs @@ -62,11 +62,6 @@ impl Registers { } pub fn put_tx_frame(&self, bufidx: usize, header: &Header, buffer: &[u8]) { - // Fill level - do we have a msg? - //if self.regs.rxfs(fifonr).read().ffl() < 1 { return None; } - - //let read_idx = self.regs.rxfs(fifonr).read().fgi(); - let mailbox = self.tx_buffer_element(bufidx); mailbox.reset(); @@ -169,9 +164,6 @@ impl Registers { } #[inline] - //fn abort_pending_mailbox(&mut self, idx: Mailbox, pending: PTX) -> Option -//where - // PTX: FnOnce(Mailbox, TxFrameHeader, &[u32]) -> R, pub fn abort_pending_mailbox_generic(&self, bufidx: usize) -> Option { if self.abort(bufidx) { let mailbox = self.tx_buffer_element(bufidx); @@ -187,7 +179,6 @@ impl Registers { return None; } - //let tx_ram = self.tx_msg_ram(); let mut data = [0u8; 64]; data_from_tx_buffer(&mut data, mailbox, len as usize); @@ -205,9 +196,6 @@ impl Registers { } } - /// As Transmit, but if there is a pending frame, `pending` will be called so that the frame can - /// be preserved. - //pub fn transmit_preserve( pub fn write(&self, frame: &F) -> nb::Result, Infallible> { let queue_is_full = self.tx_queue_is_full(); @@ -459,8 +447,6 @@ impl Registers { /// parameter to this method. #[inline] pub fn set_nominal_bit_timing(&mut self, btr: NominalBitTiming) { - //self.control.config.nbtr = btr; - self.regs.nbtp().write(|w| { w.set_nbrp(btr.nbrp() - 1); w.set_ntseg1(btr.ntseg1() - 1); @@ -473,8 +459,6 @@ impl Registers { /// This is not used when frame_transmit is set to anything other than AllowFdCanAndBRS. #[inline] pub fn set_data_bit_timing(&mut self, btr: DataBitTiming) { - //self.control.config.dbtr = btr; - self.regs.dbtp().write(|w| { w.set_dbrp(btr.dbrp() - 1); w.set_dtseg1(btr.dtseg1() - 1); @@ -492,7 +476,6 @@ impl Registers { #[inline] pub fn set_automatic_retransmit(&mut self, enabled: bool) { self.regs.cccr().modify(|w| w.set_dar(!enabled)); - //self.control.config.automatic_retransmit = enabled; } /// Configures the transmit pause feature. See @@ -500,21 +483,18 @@ impl Registers { #[inline] pub fn set_transmit_pause(&mut self, enabled: bool) { self.regs.cccr().modify(|w| w.set_txp(!enabled)); - //self.control.config.transmit_pause = enabled; } /// Configures non-iso mode. See [`FdCanConfig::set_non_iso_mode`] #[inline] pub fn set_non_iso_mode(&mut self, enabled: bool) { self.regs.cccr().modify(|w| w.set_niso(enabled)); - //self.control.config.non_iso_mode = enabled; } /// Configures edge filtering. See [`FdCanConfig::set_edge_filtering`] #[inline] pub fn set_edge_filtering(&mut self, enabled: bool) { self.regs.cccr().modify(|w| w.set_efbi(enabled)); - //self.control.config.edge_filtering = enabled; } /// Configures frame transmission mode. See @@ -534,16 +514,12 @@ impl Registers { #[cfg(not(stm32h7))] w.set_brse(brse); }); - - //self.control.config.frame_transmit = fts; } /// Sets the protocol exception handling on/off #[inline] pub fn set_protocol_exception_handling(&mut self, enabled: bool) { self.regs.cccr().modify(|w| w.set_pxhd(!enabled)); - - //self.control.config.protocol_exception_handling = enabled; } /// Configures and resets the timestamp counter @@ -567,8 +543,6 @@ impl Registers { w.set_tcp(tcp); w.set_tss(tss); }); - - //self.control.config.timestamp_source = select; } #[cfg(not(stm32h7))] @@ -696,22 +670,6 @@ fn data_from_tx_buffer(buffer: &mut [u8], mailbox: &TxBufferElement, len: usize) } } -impl From<&RxFifoElement> for ClassicFrame { - fn from(mailbox: &RxFifoElement) -> Self { - let header_reg = mailbox.header.read(); - - let id = make_id(header_reg.id().bits(), header_reg.xtd().bits()); - let dlc = header_reg.to_data_length().len(); - let len = dlc as usize; - - let mut buffer: [u8; 64] = [0; 64]; - data_from_fifo(&mut buffer, mailbox, len); - let data = ClassicData::new(&buffer[0..len]); - let header = Header::new(id, dlc, header_reg.rtr().bits()); - ClassicFrame::new(header, data.unwrap()) - } -} - fn extract_frame(mailbox: &RxFifoElement, buffer: &mut [u8]) -> Option<(Header, u16)> { let header_reg = mailbox.header.read(); diff --git a/embassy-stm32/src/can/frame.rs b/embassy-stm32/src/can/frame.rs index 59b9fb08c..00a441db6 100644 --- a/embassy-stm32/src/can/frame.rs +++ b/embassy-stm32/src/can/frame.rs @@ -292,8 +292,6 @@ pub struct FdFrame { } impl FdFrame { - pub(crate) const MAX_DATA_LEN: usize = 64; - /// Create a new CAN classic Frame pub fn new(can_header: Header, data: FdData) -> FdFrame { FdFrame { can_header, data }