Merge pull request #2988 from de-vri-es/bxcan-tx-fifo-scheduling

embassy_stm32: implement optional FIFO scheduling for outgoing frames

2 files changed, 98 insertions, 22 deletions

diff --git a/embassy-stm32/src/can/bxcan/mod.rs b/embassy-stm32/src/can/bxcan/mod.rs
index 912634b84..0ac4cdab6 100644
--- a/embassy-stm32/src/can/bxcan/mod.rs
+++ b/embassy-stm32/src/can/bxcan/mod.rs
@@ -133,7 +133,7 @@ impl<T: Instance> CanConfig<'_, T> {
         self
     }
 
-    /// Enables or disables automatic retransmission of messages.
+    /// Enables or disables automatic retransmission of frames.
     ///
     /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame
     /// until it can be sent. Otherwise, it will try only once to send each frame.
@@ -298,6 +298,23 @@ impl<'d, T: Instance> Can<'d, T> {
         T::regs().ier().modify(|i| i.set_slkie(false));
     }
 
+    /// Enable FIFO scheduling of outgoing frames.
+    ///
+    /// If this is enabled, frames will be transmitted in the order that they are passed to
+    /// [`write()`][Self::write] or [`try_write()`][Self::try_write()].
+    ///
+    /// If this is disabled, frames are transmitted in order of priority.
+    ///
+    /// FIFO scheduling is disabled by default.
+    pub fn set_tx_fifo_scheduling(&mut self, enabled: bool) {
+        Registers(T::regs()).set_tx_fifo_scheduling(enabled)
+    }
+
+    /// Checks if FIFO scheduling of outgoing frames is enabled.
+    pub fn tx_fifo_scheduling_enabled(&self) -> bool {
+        Registers(T::regs()).tx_fifo_scheduling_enabled()
+    }
+
     /// Queues the message to be sent.
     ///
     /// If the TX queue is full, this will wait until there is space, therefore exerting backpressure.
@@ -307,7 +324,13 @@ impl<'d, T: Instance> Can<'d, T> {
 
     /// Attempts to transmit a frame without blocking.
     ///
-    /// Returns [Err(TryWriteError::Full)] if all transmit mailboxes are full.
+    /// Returns [Err(TryWriteError::Full)] if the frame can not be queued for transmission now.
+    ///
+    /// If FIFO scheduling is enabled, any empty mailbox will be used.
+    ///
+    /// Otherwise, the frame will only be accepted if there is no frame with the same priority already queued.
+    /// This is done to work around a hardware limitation that could lead to out-of-order delivery
+    /// of frames with the same priority.
     pub fn try_write(&mut self, frame: &Frame) -> Result<TransmitStatus, TryWriteError> {
         self.split().0.try_write(frame)
     }
@@ -318,6 +341,11 @@ impl<'d, T: Instance> Can<'d, T> {
     }
 
     /// Waits until any of the transmit mailboxes become empty
+    ///
+    /// Note that [`Self::try_write()`] may fail with [`TryWriteError::Full`],
+    /// even after the future returned by this function completes.
+    /// This will happen if FIFO scheduling of outgoing frames is not enabled,
+    /// and a frame with equal priority is already queued for transmission.
     pub async fn flush_any(&self) {
         CanTx::<T>::flush_any_inner().await
     }
@@ -465,7 +493,13 @@ impl<'d, T: Instance> CanTx<'d, T> {
 
     /// Attempts to transmit a frame without blocking.
     ///
-    /// Returns [Err(TryWriteError::Full)] if all transmit mailboxes are full.
+    /// Returns [Err(TryWriteError::Full)] if the frame can not be queued for transmission now.
+    ///
+    /// If FIFO scheduling is enabled, any empty mailbox will be used.
+    ///
+    /// Otherwise, the frame will only be accepted if there is no frame with the same priority already queued.
+    /// This is done to work around a hardware limitation that could lead to out-of-order delivery
+    /// of frames with the same priority.
     pub fn try_write(&mut self, frame: &Frame) -> Result<TransmitStatus, TryWriteError> {
         Registers(T::regs()).transmit(frame).map_err(|_| TryWriteError::Full)
     }
@@ -505,6 +539,11 @@ impl<'d, T: Instance> CanTx<'d, T> {
     }
 
     /// Waits until any of the transmit mailboxes become empty
+    ///
+    /// Note that [`Self::try_write()`] may fail with [`TryWriteError::Full`],
+    /// even after the future returned by this function completes.
+    /// This will happen if FIFO scheduling of outgoing frames is not enabled,
+    /// and a frame with equal priority is already queued for transmission.
     pub async fn flush_any(&self) {
         Self::flush_any_inner().await
     }
diff --git a/embassy-stm32/src/can/bxcan/registers.rs b/embassy-stm32/src/can/bxcan/registers.rs
index 446f3ad6f..ad27e0744 100644
--- a/embassy-stm32/src/can/bxcan/registers.rs
+++ b/embassy-stm32/src/can/bxcan/registers.rs
@@ -181,46 +181,85 @@ impl Registers {
         None
     }
 
+    /// Enables or disables FIFO scheduling of outgoing mailboxes.
+    ///
+    /// If this is enabled, mailboxes are scheduled based on the time when the transmit request bit of the mailbox was set.
+    ///
+    /// If this is disabled, mailboxes are scheduled based on the priority of the frame in the mailbox.
+    pub fn set_tx_fifo_scheduling(&mut self, enabled: bool) {
+        self.0.mcr().modify(|w| w.set_txfp(enabled))
+    }
+
+    /// Checks if FIFO scheduling of outgoing mailboxes is enabled.
+    pub fn tx_fifo_scheduling_enabled(&self) -> bool {
+        self.0.mcr().read().txfp()
+    }
+
     /// Puts a CAN frame in a transmit mailbox for transmission on the bus.
     ///
-    /// Frames are transmitted to the bus based on their priority (see [`FramePriority`]).
-    /// Transmit order is preserved for frames with identical priority.
+    /// The behavior of this function depends on wheter or not FIFO scheduling is enabled.
+    /// See [`Self::set_tx_fifo_scheduling()`] and [`Self::tx_fifo_scheduling_enabled()`].
+    ///
+    /// # Priority based scheduling
+    ///
+    /// If FIFO scheduling is disabled, frames are transmitted to the bus based on their
+    /// priority (see [`FramePriority`]). Transmit order is preserved for frames with identical
+    /// priority.
     ///
     /// If all transmit mailboxes are full, and `frame` has a higher priority than the
     /// lowest-priority message in the transmit mailboxes, transmission of the enqueued frame is
     /// cancelled and `frame` is enqueued instead. The frame that was replaced is returned as
     /// [`TransmitStatus::dequeued_frame`].
+    ///
+    /// # FIFO scheduling
+    ///
+    /// If FIFO scheduling is enabled, frames are transmitted in the order that they are passed to this function.
+    ///
+    /// If all transmit mailboxes are full, this function returns [`nb::Error::WouldBlock`].
     pub fn transmit(&mut self, frame: &Frame) -> nb::Result<TransmitStatus, Infallible> {
+        // Check if FIFO scheduling is enabled.
+        let fifo_scheduling = self.0.mcr().read().txfp();
+
         // Get the index of the next free mailbox or the one with the lowest priority.
         let tsr = self.0.tsr().read();
         let idx = tsr.code() as usize;
 
         let frame_is_pending = !tsr.tme(0) || !tsr.tme(1) || !tsr.tme(2);
-        let pending_frame = if frame_is_pending {
-            // High priority frames are transmitted first by the mailbox system.
-            // Frames with identical identifier shall be transmitted in FIFO order.
-            // The controller schedules pending frames of same priority based on the
-            // mailbox index instead. As a workaround check all pending mailboxes
-            // and only accept higher priority frames.
+        let all_frames_are_pending = !tsr.tme(0) && !tsr.tme(1) && !tsr.tme(2);
+
+        let pending_frame;
+        if fifo_scheduling && all_frames_are_pending {
+            // FIFO scheduling is enabled and all mailboxes are full.
+            // We will not drop a lower priority frame, we just report WouldBlock.
+            return Err(nb::Error::WouldBlock);
+        } else if !fifo_scheduling && frame_is_pending {
+            // Priority scheduling is enabled and alteast one mailbox is full.
+            //
+            // In this mode, the peripheral transmits high priority frames first.
+            // Frames with identical priority should be transmitted in FIFO order,
+            // but the controller schedules pending frames of same priority based on the
+            // mailbox index. As a workaround check all pending mailboxes and only accept
+            // frames with a different priority.
             self.check_priority(0, frame.id().into())?;
             self.check_priority(1, frame.id().into())?;
             self.check_priority(2, frame.id().into())?;
 
-            let all_frames_are_pending = !tsr.tme(0) && !tsr.tme(1) && !tsr.tme(2);
             if all_frames_are_pending {
                 // No free mailbox is available. This can only happen when three frames with
                 // ascending priority (descending IDs) were requested for transmission and all
                 // of them are blocked by bus traffic with even higher priority.
                 // To prevent a priority inversion abort and replace the lowest priority frame.
-                self.read_pending_mailbox(idx)
+                pending_frame = self.read_pending_mailbox(idx);
             } else {
                 // There was a free mailbox.
-                None
+                pending_frame = None;
             }
         } else {
-            // All mailboxes are available: Send frame without performing any checks.
-            None
-        };
+            // Either we have FIFO scheduling and at-least one free mailbox,
+            // or we have priority scheduling and all mailboxes are free.
+            // No further checks are needed.
+            pending_frame = None
+        }
 
         self.write_mailbox(idx, frame);
 
@@ -237,18 +276,16 @@ impl Registers {
     }
 
     /// Returns `Ok` when the mailbox is free or if it contains pending frame with a
-    /// lower priority (higher ID) than the identifier `id`.
+    /// different priority from the identifier `id`.
     fn check_priority(&self, idx: usize, id: IdReg) -> nb::Result<(), Infallible> {
         // Read the pending frame's id to check its priority.
         assert!(idx < 3);
         let tir = &self.0.tx(idx).tir().read();
-        //let tir = &can.tx[idx].tir.read();
 
         // Check the priority by comparing the identifiers. But first make sure the
         // frame has not finished the transmission (`TXRQ` == 0) in the meantime.
-        if tir.txrq() && id <= IdReg::from_register(tir.0) {
-            // There's a mailbox whose priority is higher or equal
-            // the priority of the new frame.
+        if tir.txrq() && id == IdReg::from_register(tir.0) {
+            // There's a mailbox whose priority is equal to the priority of the new frame.
             return Err(nb::Error::WouldBlock);
         }