3 files changed, 167 insertions, 129 deletions

diff --git a/embassy-stm32/src/can/fd/config.rs b/embassy-stm32/src/can/fd/config.rs
index f2401d92f..adaffe9cc 100644
--- a/embassy-stm32/src/can/fd/config.rs
+++ b/embassy-stm32/src/can/fd/config.rs
@@ -287,6 +287,33 @@ impl Default for GlobalFilter {
     }
 }
 
+/// TX buffer operation mode
+#[derive(Clone, Copy, PartialEq, Eq, Debug)]
+pub enum TxBufferMode {
+    /// TX FIFO operation - In this mode CAN frames are trasmitted strictly in write order.
+    Fifo,
+    /// TX queue operation - In this mode CAN frames are transmitted according to CAN priority.
+    Queue,
+}
+
+impl From<TxBufferMode> for crate::pac::can::vals::Tfqm {
+    fn from(value: TxBufferMode) -> Self {
+        match value {
+            TxBufferMode::Queue => Self::QUEUE,
+            TxBufferMode::Fifo => Self::FIFO,
+        }
+    }
+}
+
+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 {
@@ -327,6 +354,8 @@ pub struct FdCanConfig {
     pub timestamp_source: TimestampSource,
     /// Configures the Global Filter
     pub global_filter: GlobalFilter,
+    /// TX buffer mode (FIFO or queue)
+    pub tx_buffer_mode: TxBufferMode,
 }
 
 impl FdCanConfig {
@@ -433,6 +462,7 @@ impl Default for FdCanConfig {
             clock_divider: ClockDivider::_1,
             timestamp_source: TimestampSource::None,
             global_filter: GlobalFilter::default(),
+            tx_buffer_mode: TxBufferMode::Queue,
         }
     }
 }
diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs
index e87a3c213..8ec09ac12 100644
--- a/embassy-stm32/src/can/fd/peripheral.rs
+++ b/embassy-stm32/src/can/fd/peripheral.rs
@@ -22,6 +22,7 @@ enum LoopbackMode {
 pub struct Registers {
     pub regs: &'static crate::pac::can::Fdcan,
     pub msgram: &'static crate::pac::fdcanram::Fdcanram,
+    pub msg_ram_offset: usize,
 }
 
 impl Registers {
@@ -114,6 +115,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)
@@ -164,7 +171,7 @@ impl Registers {
     }
 
     #[inline]
-    pub fn abort_pending_mailbox_generic<F: embedded_can::Frame>(&self, bufidx: usize) -> Option<F> {
+    fn abort_pending_mailbox<F: embedded_can::Frame>(&self, bufidx: usize) -> Option<F> {
         if self.abort(bufidx) {
             let mailbox = self.tx_buffer_element(bufidx);
 
@@ -197,19 +204,20 @@ 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))
+                (0, self.abort_pending_mailbox(0))
             } else if self.is_available(1, id) {
-                (1, self.abort_pending_mailbox_generic(1))
+                (1, self.abort_pending_mailbox(1))
             } else if self.is_available(2, id) {
-                (2, self.abort_pending_mailbox_generic(2))
+                (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?
@@ -287,7 +295,6 @@ impl Registers {
     pub fn into_config_mode(mut self, _config: FdCanConfig) {
         self.set_power_down_mode(false);
         self.enter_init_mode();
-
         self.reset_msg_ram();
 
         // check the FDCAN core matches our expections
@@ -300,12 +307,31 @@ impl Registers {
             "Error reading endianness test value from FDCAN core"
         );
 
-        // Framework specific settings are set here
+        /*
+        for fid in 0..crate::can::message_ram::STANDARD_FILTER_MAX {
+            self.set_standard_filter((fid as u8).into(), StandardFilter::disable());
+        }
+        for fid in 0..Ecrate::can::message_ram::XTENDED_FILTER_MAX {
+            self.set_extended_filter(fid.into(), ExtendedFilter::disable());
+        }
+        */
+    }
 
-        // set TxBuffer to Queue Mode
-        self.regs
-            .txbc()
-            .write(|w| w.set_tfqm(crate::pac::can::vals::Tfqm::QUEUE));
+    /// Disables the CAN interface and returns back the raw peripheral it was created from.
+    #[inline]
+    pub fn free(mut self) {
+        //self.disable_interrupts(Interrupts::all());
+
+        //TODO check this!
+        self.enter_init_mode();
+        self.set_power_down_mode(true);
+        //self.control.instance
+    }
+
+    /// Applies the settings of a new FdCanConfig See [`FdCanConfig`]
+    #[inline]
+    pub fn apply_config(&mut self, config: FdCanConfig) {
+        self.set_tx_buffer_mode(config.tx_buffer_mode);
 
         // set standard filters list size to 28
         // set extended filters list size to 8
@@ -328,30 +354,29 @@ impl Registers {
                 .modify(|w| w.set_lse(crate::can::fd::message_ram::EXTENDED_FILTER_MAX));
         }
 
-        /*
-        for fid in 0..crate::can::message_ram::STANDARD_FILTER_MAX {
-            self.set_standard_filter((fid as u8).into(), StandardFilter::disable());
-        }
-        for fid in 0..Ecrate::can::message_ram::XTENDED_FILTER_MAX {
-            self.set_extended_filter(fid.into(), ExtendedFilter::disable());
-        }
-        */
-    }
-
-    /// Disables the CAN interface and returns back the raw peripheral it was created from.
-    #[inline]
-    pub fn free(mut self) {
-        //self.disable_interrupts(Interrupts::all());
+        self.configure_msg_ram();
 
-        //TODO check this!
-        self.enter_init_mode();
-        self.set_power_down_mode(true);
-        //self.control.instance
-    }
+        // Enable timestamping
+        #[cfg(not(stm32h7))]
+        self.regs
+            .tscc()
+            .write(|w| w.set_tss(stm32_metapac::can::vals::Tss::INCREMENT));
+        #[cfg(stm32h7)]
+        self.regs.tscc().write(|w| w.set_tss(0x01));
+
+        // this isn't really documented in the reference manual
+        // but corresponding txbtie bit has to be set for the TC (TxComplete) interrupt to fire
+        self.regs.txbtie().write(|w| w.0 = 0xffff_ffff);
+        self.regs.ie().modify(|w| {
+            w.set_rfne(0, true); // Rx Fifo 0 New Msg
+            w.set_rfne(1, true); // Rx Fifo 1 New Msg
+            w.set_tce(true); //  Tx Complete
+        });
+        self.regs.ile().modify(|w| {
+            w.set_eint0(true); // Interrupt Line 0
+            w.set_eint1(true); // Interrupt Line 1
+        });
 
-    /// Applies the settings of a new FdCanConfig See [`FdCanConfig`]
-    #[inline]
-    pub fn apply_config(&mut self, config: FdCanConfig) {
         self.set_data_bit_timing(config.dbtr);
         self.set_nominal_bit_timing(config.nbtr);
         self.set_automatic_retransmit(config.automatic_retransmit);
@@ -499,6 +524,12 @@ impl Registers {
         self.regs.cccr().modify(|w| w.set_efbi(enabled));
     }
 
+    /// Configures TX Buffer Mode
+    #[inline]
+    pub fn set_tx_buffer_mode(&mut self, tbm: TxBufferMode) {
+        self.regs.txbc().write(|w| w.set_tfqm(tbm.into()));
+    }
+
     /// Configures frame transmission mode. See
     /// [`FdCanConfig::set_frame_transmit`]
     #[inline]
@@ -593,6 +624,71 @@ impl Registers {
             w.set_rrfe(filter.reject_remote_extended_frames);
         });
     }
+
+    #[cfg(not(stm32h7))]
+    fn configure_msg_ram(&mut self) {}
+
+    #[cfg(stm32h7)]
+    fn configure_msg_ram(&mut self) {
+        let r = self.regs;
+
+        use crate::can::fd::message_ram::*;
+        //use fdcan::message_ram::*;
+        let mut offset_words = self.msg_ram_offset as u16;
+
+        // 11-bit filter
+        r.sidfc().modify(|w| w.set_flssa(offset_words));
+        offset_words += STANDARD_FILTER_MAX as u16;
+
+        // 29-bit filter
+        r.xidfc().modify(|w| w.set_flesa(offset_words));
+        offset_words += 2 * EXTENDED_FILTER_MAX as u16;
+
+        // Rx FIFO 0 and 1
+        for i in 0..=1 {
+            r.rxfc(i).modify(|w| {
+                w.set_fsa(offset_words);
+                w.set_fs(RX_FIFO_MAX);
+                w.set_fwm(RX_FIFO_MAX);
+            });
+            offset_words += 18 * RX_FIFO_MAX as u16;
+        }
+
+        // Rx buffer - see below
+        // Tx event FIFO
+        r.txefc().modify(|w| {
+            w.set_efsa(offset_words);
+            w.set_efs(TX_EVENT_MAX);
+            w.set_efwm(TX_EVENT_MAX);
+        });
+        offset_words += 2 * TX_EVENT_MAX as u16;
+
+        // Tx buffers
+        r.txbc().modify(|w| {
+            w.set_tbsa(offset_words);
+            w.set_tfqs(TX_FIFO_MAX);
+        });
+        offset_words += 18 * TX_FIFO_MAX as u16;
+
+        // Rx Buffer - not used
+        r.rxbc().modify(|w| {
+            w.set_rbsa(offset_words);
+        });
+
+        // TX event FIFO?
+        // Trigger memory?
+
+        // Set the element sizes to 16 bytes
+        r.rxesc().modify(|w| {
+            w.set_rbds(0b111);
+            for i in 0..=1 {
+                w.set_fds(i, 0b111);
+            }
+        });
+        r.txesc().modify(|w| {
+            w.set_tbds(0b111);
+        })
+    }
 }
 
 fn make_id(id: u32, extended: bool) -> embedded_can::Id {
diff --git a/embassy-stm32/src/can/fdcan.rs b/embassy-stm32/src/can/fdcan.rs
index 77db774fc..20d00ccb5 100644
--- a/embassy-stm32/src/can/fdcan.rs
+++ b/embassy-stm32/src/can/fdcan.rs
@@ -184,43 +184,20 @@ impl<'d, T: Instance> FdcanConfigurator<'d, T> {
         T::enable_and_reset();
 
         let mut config = crate::can::fd::config::FdCanConfig::default();
+        config.timestamp_source = TimestampSource::Prescaler(TimestampPrescaler::_1);
         T::registers().into_config_mode(config);
 
         rx.set_as_af(rx.af_num(), AFType::Input);
         tx.set_as_af(tx.af_num(), AFType::OutputPushPull);
 
-        T::configure_msg_ram();
         unsafe {
-            // Enable timestamping
-            #[cfg(not(stm32h7))]
-            T::regs()
-                .tscc()
-                .write(|w| w.set_tss(stm32_metapac::can::vals::Tss::INCREMENT));
-            #[cfg(stm32h7)]
-            T::regs().tscc().write(|w| w.set_tss(0x01));
-            config.timestamp_source = TimestampSource::Prescaler(TimestampPrescaler::_1);
-
             T::IT0Interrupt::unpend(); // Not unsafe
             T::IT0Interrupt::enable();
 
             T::IT1Interrupt::unpend(); // Not unsafe
             T::IT1Interrupt::enable();
-
-            // this isn't really documented in the reference manual
-            // but corresponding txbtie bit has to be set for the TC (TxComplete) interrupt to fire
-            T::regs().txbtie().write(|w| w.0 = 0xffff_ffff);
         }
 
-        T::regs().ie().modify(|w| {
-            w.set_rfne(0, true); // Rx Fifo 0 New Msg
-            w.set_rfne(1, true); // Rx Fifo 1 New Msg
-            w.set_tce(true); //  Tx Complete
-        });
-        T::regs().ile().modify(|w| {
-            w.set_eint0(true); // Interrupt Line 0
-            w.set_eint1(true); // Interrupt Line 1
-        });
-
         Self {
             config,
             instance: FdcanInstance(peri),
@@ -869,71 +846,6 @@ pub(crate) mod sealed {
         fn state() -> &'static State;
         unsafe fn mut_state() -> &'static mut State;
         fn calc_timestamp(ns_per_timer_tick: u64, ts_val: u16) -> Timestamp;
-
-        #[cfg(not(stm32h7))]
-        fn configure_msg_ram() {}
-
-        #[cfg(stm32h7)]
-        fn configure_msg_ram() {
-            let r = Self::regs();
-
-            use crate::can::fd::message_ram::*;
-            //use fdcan::message_ram::*;
-            let mut offset_words = Self::MSG_RAM_OFFSET as u16;
-
-            // 11-bit filter
-            r.sidfc().modify(|w| w.set_flssa(offset_words));
-            offset_words += STANDARD_FILTER_MAX as u16;
-
-            // 29-bit filter
-            r.xidfc().modify(|w| w.set_flesa(offset_words));
-            offset_words += 2 * EXTENDED_FILTER_MAX as u16;
-
-            // Rx FIFO 0 and 1
-            for i in 0..=1 {
-                r.rxfc(i).modify(|w| {
-                    w.set_fsa(offset_words);
-                    w.set_fs(RX_FIFO_MAX);
-                    w.set_fwm(RX_FIFO_MAX);
-                });
-                offset_words += 18 * RX_FIFO_MAX as u16;
-            }
-
-            // Rx buffer - see below
-            // Tx event FIFO
-            r.txefc().modify(|w| {
-                w.set_efsa(offset_words);
-                w.set_efs(TX_EVENT_MAX);
-                w.set_efwm(TX_EVENT_MAX);
-            });
-            offset_words += 2 * TX_EVENT_MAX as u16;
-
-            // Tx buffers
-            r.txbc().modify(|w| {
-                w.set_tbsa(offset_words);
-                w.set_tfqs(TX_FIFO_MAX);
-            });
-            offset_words += 18 * TX_FIFO_MAX as u16;
-
-            // Rx Buffer - not used
-            r.rxbc().modify(|w| {
-                w.set_rbsa(offset_words);
-            });
-
-            // TX event FIFO?
-            // Trigger memory?
-
-            // Set the element sizes to 16 bytes
-            r.rxesc().modify(|w| {
-                w.set_rbds(0b111);
-                for i in 0..=1 {
-                    w.set_fds(i, 0b111);
-                }
-            });
-            r.txesc().modify(|w| {
-                w.set_tbds(0b111);
-            })
-        }
     }
 }
 
@@ -957,7 +869,7 @@ macro_rules! impl_fdcan {
                 &crate::pac::$inst
             }
             fn registers() -> Registers {
-                Registers{regs: &crate::pac::$inst, msgram: &crate::pac::$msg_ram_inst}
+                Registers{regs: &crate::pac::$inst, msgram: &crate::pac::$msg_ram_inst, msg_ram_offset: Self::MSG_RAM_OFFSET}
             }
             fn ram() -> &'static crate::pac::fdcanram::Fdcanram {
                 &crate::pac::$msg_ram_inst