2 files changed, 24 insertions, 8 deletions

diff --git a/embassy-stm32/src/can/fd/config.rs b/embassy-stm32/src/can/fd/config.rs
index e959913e4..338e4979d 100644
--- a/embassy-stm32/src/can/fd/config.rs
+++ b/embassy-stm32/src/can/fd/config.rs
@@ -288,7 +288,7 @@ impl Default for GlobalFilter {
 }
 
 /// TX buffer operation mode
-#[derive(Clone, Copy, Debug)]
+#[derive(Clone, Copy, PartialEq, Eq, Debug)]
 pub enum TxBufferMode {
     /// TX FIFO operation
     Fifo,
@@ -305,6 +305,15 @@ impl From<TxBufferMode> for crate::pac::can::vals::Tfqm {
     }
 }
 
+impl From<crate::pac::can::vals::Tfqm> for TxBufferMode {
+    fn from(value: crate::pac::can::vals::Tfqm) -> Self {
+        match value {
+            crate::pac::can::vals::Tfqm::QUEUE => Self::Queue,
+            crate::pac::can::vals::Tfqm::FIFO => Self::Fifo,
+        }
+    }
+}
+
 /// FdCan Config Struct
 #[derive(Clone, Copy, Debug)]
 pub struct FdCanConfig {
diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs
index 0ebbdb3b7..2a183d2e8 100644
--- a/embassy-stm32/src/can/fd/peripheral.rs
+++ b/embassy-stm32/src/can/fd/peripheral.rs
@@ -114,6 +114,12 @@ impl Registers {
         self.regs.txfqs().read().tfqf()
     }
 
+    /// Returns the current TX buffer operation mode (queue or FIFO)
+    #[inline]
+    pub fn tx_queue_mode(&self) -> TxBufferMode {
+        self.regs.txbc().read().tfqm().into()
+    }
+
     #[inline]
     pub fn has_pending_frame(&self, idx: usize) -> bool {
         self.regs.txbrp().read().trp(idx)
@@ -197,13 +203,14 @@ impl Registers {
     }
 
     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();
-
-        // If the queue is full,
-        // Discard the first slot with a lower priority message
-        let (idx, pending_frame) = if queue_is_full {
+        let (idx, pending_frame) = if self.tx_queue_is_full() {
+            if self.tx_queue_mode() == TxBufferMode::Fifo {
+                // Does not make sense to cancel a pending frame when using FIFO
+                return Err(nb::Error::WouldBlock);
+            }
+            // If the queue is full,
+            // Discard the first slot with a lower priority message
+            let id = frame.header().id();
             if self.is_available(0, id) {
                 (0, self.abort_pending_mailbox_generic(0))
             } else if self.is_available(1, id) {