stm32: can: fd: refactor out some duplicate code

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<F: CanHeader>(&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<PTX, R>(&mut self, idx: Mailbox, pending: PTX) -> Option<R>
-    pub fn abort_pending_mailbox(&self, bufidx: usize) -> Option<ClassicFrame>
 //where
     //    PTX: FnOnce(Mailbox, TxFrameHeader, &[u32]) -> R,
-    {
+    pub fn abort_pending_mailbox_generic<F: embedded_can::Frame>(&self, bufidx: usize) -> Option<F> {
         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<PTX, R>(&mut self, idx: Mailbox, pending: PTX) -> Option<R>
-    pub fn abort_pending_fd_mailbox(&self, bufidx: usize) -> Option<FdFrame>
-//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,41 +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<PTX, P>(
-    pub fn write_classic(&self, frame: &ClassicFrame) -> nb::Result<Option<ClassicFrame>, 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_mailbox(0))
-            } else if self.is_available(1, id) {
-                (1, self.abort_pending_mailbox(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<PTX, P>(
-    pub fn write_fd(&self, frame: &FdFrame) -> nb::Result<Option<FdFrame>, Infallible> {
+    pub fn write<F: embedded_can::Frame + CanHeader>(&self, frame: &F) -> nb::Result<Option<F>, Infallible> {
         let queue_is_full = self.tx_queue_is_full();
 
         let id = frame.header().id();
@@ -315,11 +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_fd_mailbox(0))
+                (0, self.abort_pending_mailbox_generic(0))
             } else if self.is_available(1, id) {
-                (1, self.abort_pending_fd_mailbox(1))
+                (1, self.abort_pending_mailbox_generic(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<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> 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<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> 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::<T>().await
+        T::state().rx_mode.read_classic::<T>().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::<T>().await
+        T::state().rx_mode.read_classic::<T>().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<T: Instance>(&self) -> Result<(ClassicFrame, Timestamp), BusError> {
+        async fn read<T: Instance, F: CanHeader>(&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<T: Instance>(&self) -> Result<(FdFrame, Timestamp), BusError> {
-            poll_fn(|cx| {
-                T::state().err_waker.register(cx.waker());
-                self.register(cx.waker());
+        pub async fn read_classic<T: Instance>(&self) -> Result<(ClassicFrame, Timestamp), BusError> {
+            self.read::<T, _>().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<T: Instance>(&self) -> Result<(FdFrame, Timestamp), BusError> {
+            self.read::<T, _>().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<T: Instance>(&self, frame: &ClassicFrame) -> Option<ClassicFrame> {
+        async fn write_generic<T: Instance, F: embedded_can::Frame + CanHeader>(&self, frame: &F) -> Option<F> {
             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);
                 }
 
@@ -785,19 +774,16 @@ 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_fd<T: Instance>(&self, frame: &FdFrame) -> Option<FdFrame> {
-            poll_fn(|cx| {
-                self.register(cx.waker());
-
-                if let Ok(dropped) = T::registers().write_fd(frame) {
-                    return Poll::Ready(dropped);
-                }
+        pub async fn write<T: Instance>(&self, frame: &ClassicFrame) -> Option<ClassicFrame> {
+            self.write_generic::<T, _>(frame).await
+        }
 
-                // Couldn't replace any lower priority frames.  Need to wait for some mailboxes
-                // to clear.
-                Poll::Pending
-            })
-            .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<T: Instance>(&self, frame: &FdFrame) -> Option<FdFrame> {
+            self.write_generic::<T, _>(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<Self>;
+
+    /// 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<Self> {
+        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<Self> {
+        Some(Self::new(header, FdData::new(data)?))
+    }
+
+    fn header(&self) -> &Header {
+        self.header()
+    }
+}