BXCAN: Register access into new Registers struct.

4 files changed, 452 insertions, 367 deletions

diff --git a/embassy-stm32/src/can/bx/mod.rs b/embassy-stm32/src/can/bx/mod.rs
index 5ea0471c9..9b6ebf5a4 100644
--- a/embassy-stm32/src/can/bx/mod.rs
+++ b/embassy-stm32/src/can/bx/mod.rs
@@ -43,7 +43,35 @@ pub type Data = crate::can::frame::ClassicData;
 /// CAN Frame
 pub type Frame = crate::can::frame::ClassicFrame;
 
+use crate::can::_version::Envelope;
 use crate::can::bx::filter::MasterFilters;
+use crate::can::enums::BusError;
+use crate::pac::can::vals::Lec;
+
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+pub(crate) enum RxFifo {
+    Fifo0,
+    Fifo1,
+}
+
+trait IntoBusError {
+    fn into_bus_err(self) -> Option<BusError>;
+}
+
+impl IntoBusError for Lec {
+    fn into_bus_err(self) -> Option<BusError> {
+        match self {
+            Lec::STUFF => Some(BusError::Stuff),
+            Lec::FORM => Some(BusError::Form),
+            Lec::ACK => Some(BusError::Acknowledge),
+            Lec::BITRECESSIVE => Some(BusError::BitRecessive),
+            Lec::BITDOMINANT => Some(BusError::BitDominant),
+            Lec::CRC => Some(BusError::Crc),
+            Lec::CUSTOM => Some(BusError::Software),
+            _ => None,
+        }
+    }
+}
 
 /// A bxCAN peripheral instance.
 ///
@@ -233,6 +261,376 @@ impl PartialOrd for IdReg {
     }
 }
 
+pub(crate) struct Registers {
+    pub canregs: crate::pac::can::Can,
+}
+
+impl Registers {
+    fn enter_init_mode(&mut self) {
+        self.canregs.mcr().modify(|reg| {
+            reg.set_sleep(false);
+            reg.set_inrq(true);
+        });
+        loop {
+            let msr = self.canregs.msr().read();
+            if !msr.slak() && msr.inak() {
+                break;
+            }
+        }
+    }
+
+    // Leaves initialization mode, enters sleep mode.
+    fn leave_init_mode(&mut self) {
+        self.canregs.mcr().modify(|reg| {
+            reg.set_sleep(true);
+            reg.set_inrq(false);
+        });
+        loop {
+            let msr = self.canregs.msr().read();
+            if msr.slak() && !msr.inak() {
+                break;
+            }
+        }
+    }
+
+    fn set_bit_timing(&mut self, bt: crate::can::util::NominalBitTiming) {
+        let prescaler = u16::from(bt.prescaler) & 0x1FF;
+        let seg1 = u8::from(bt.seg1);
+        let seg2 = u8::from(bt.seg2) & 0x7F;
+        let sync_jump_width = u8::from(bt.sync_jump_width) & 0x7F;
+        self.canregs.btr().modify(|reg| {
+            reg.set_brp(prescaler - 1);
+            reg.set_ts(0, seg1 - 1);
+            reg.set_ts(1, seg2 - 1);
+            reg.set_sjw(sync_jump_width - 1);
+        });
+    }
+
+    /// Enables or disables silent mode: Disconnects the TX signal from the pin.
+    pub fn set_silent(&self, enabled: bool) {
+        let mode = match enabled {
+            false => stm32_metapac::can::vals::Silm::NORMAL,
+            true => stm32_metapac::can::vals::Silm::SILENT,
+        };
+        self.canregs.btr().modify(|reg| reg.set_silm(mode));
+    }
+
+    /// Enables or disables automatic retransmission of messages.
+    ///
+    /// 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.
+    ///
+    /// Automatic retransmission is enabled by default.
+    pub fn set_automatic_retransmit(&self, enabled: bool) {
+        self.canregs.mcr().modify(|reg| reg.set_nart(enabled));
+    }
+
+    /// Enables or disables loopback mode: Internally connects the TX and RX
+    /// signals together.
+    pub fn set_loopback(&self, enabled: bool) {
+        self.canregs.btr().modify(|reg| reg.set_lbkm(enabled));
+    }
+
+    /// Configures the automatic wake-up feature.
+    ///
+    /// This is turned off by default.
+    ///
+    /// When turned on, an incoming frame will cause the peripheral to wake up from sleep and
+    /// receive the frame. If enabled, [`Interrupt::Wakeup`] will also be triggered by the incoming
+    /// frame.
+    #[allow(dead_code)]
+    pub fn set_automatic_wakeup(&mut self, enabled: bool) {
+        self.canregs.mcr().modify(|reg| reg.set_awum(enabled));
+    }
+
+    /// Leaves initialization mode and enables the peripheral (non-blocking version).
+    ///
+    /// Usually, it is recommended to call [`CanConfig::enable`] instead. This method is only needed
+    /// if you want non-blocking initialization.
+    ///
+    /// If this returns [`WouldBlock`][nb::Error::WouldBlock], the peripheral will enable itself
+    /// in the background. The peripheral is enabled and ready to use when this method returns
+    /// successfully.
+    pub fn enable_non_blocking(&mut self) -> nb::Result<(), Infallible> {
+        let msr = self.canregs.msr().read();
+        if msr.slak() {
+            self.canregs.mcr().modify(|reg| {
+                reg.set_abom(true);
+                reg.set_sleep(false);
+            });
+            Err(nb::Error::WouldBlock)
+        } else {
+            Ok(())
+        }
+    }
+
+    /// Puts the peripheral in a sleep mode to save power.
+    ///
+    /// While in sleep mode, an incoming CAN frame will trigger [`Interrupt::Wakeup`] if enabled.
+    #[allow(dead_code)]
+    pub fn sleep(&mut self) {
+        self.canregs.mcr().modify(|reg| {
+            reg.set_sleep(true);
+            reg.set_inrq(false);
+        });
+        loop {
+            let msr = self.canregs.msr().read();
+            if msr.slak() && !msr.inak() {
+                break;
+            }
+        }
+    }
+
+    /// Disables the CAN interface.
+    ///
+    /// The peripheral is disabled by setting `RESET` in `CAN_MCR`, which causes the peripheral to
+    /// enter sleep mode.
+    pub fn reset(&self) {
+        self.canregs.mcr().write(|reg| reg.set_reset(true));
+    }
+
+    /// Wakes up from sleep mode.
+    ///
+    /// Note that this will not trigger [`Interrupt::Wakeup`], only reception of an incoming CAN
+    /// frame will cause that interrupt.
+    #[allow(dead_code)]
+    pub fn wakeup(&mut self) {
+        self.canregs.mcr().modify(|reg| {
+            reg.set_sleep(false);
+            reg.set_inrq(false);
+        });
+        loop {
+            let msr = self.canregs.msr().read();
+            if !msr.slak() && !msr.inak() {
+                break;
+            }
+        }
+    }
+    
+    pub fn curr_error(&self) -> Option<BusError> {
+        let err = { self.canregs.esr().read() };
+        if err.boff() {
+            return Some(BusError::BusOff);
+        } else if err.epvf() {
+            return Some(BusError::BusPassive);
+        } else if err.ewgf() {
+            return Some(BusError::BusWarning);
+        } else if let Some(err) = err.lec().into_bus_err() {
+            return Some(err);
+        }
+        None
+    }
+
+    /// 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.
+    ///
+    /// 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`].
+    pub fn transmit(&mut self, frame: &Frame) -> nb::Result<TransmitStatus, Infallible> {
+        // Get the index of the next free mailbox or the one with the lowest priority.
+        let tsr = self.canregs.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.
+            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)
+            } else {
+                // There was a free mailbox.
+                None
+            }
+        } else {
+            // All mailboxes are available: Send frame without performing any checks.
+            None
+        };
+
+        self.write_mailbox(idx, frame);
+
+        let mailbox = match idx {
+            0 => Mailbox::Mailbox0,
+            1 => Mailbox::Mailbox1,
+            2 => Mailbox::Mailbox2,
+            _ => unreachable!(),
+        };
+        Ok(TransmitStatus {
+            dequeued_frame: pending_frame,
+            mailbox,
+        })
+    }
+
+    /// Returns `Ok` when the mailbox is free or if it contains pending frame with a
+    /// lower priority (higher ID) than 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.canregs.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.
+            return Err(nb::Error::WouldBlock);
+        }
+
+        Ok(())
+    }
+
+    fn write_mailbox(&mut self, idx: usize, frame: &Frame) {
+        debug_assert!(idx < 3);
+
+        let mb = self.canregs.tx(idx);
+        mb.tdtr().write(|w| w.set_dlc(frame.header().len() as u8));
+
+        mb.tdlr()
+            .write(|w| w.0 = u32::from_ne_bytes(frame.data()[0..4].try_into().unwrap()));
+        mb.tdhr()
+            .write(|w| w.0 = u32::from_ne_bytes(frame.data()[4..8].try_into().unwrap()));
+        let id: IdReg = frame.id().into();
+        mb.tir().write(|w| {
+            w.0 = id.0;
+            w.set_txrq(true);
+        });
+    }
+
+    fn read_pending_mailbox(&mut self, idx: usize) -> Option<Frame> {
+        if self.abort_by_index(idx) {
+            debug_assert!(idx < 3);
+
+            let mb = self.canregs.tx(idx);
+
+            let id = IdReg(mb.tir().read().0);
+            let mut data = [0xff; 8];
+            data[0..4].copy_from_slice(&mb.tdlr().read().0.to_ne_bytes());
+            data[4..8].copy_from_slice(&mb.tdhr().read().0.to_ne_bytes());
+            let len = mb.tdtr().read().dlc();
+
+            Some(Frame::new(Header::new(id.id(), len, id.rtr()), &data).unwrap())
+        } 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
+        }
+    }
+
+    /// Tries to abort a pending frame. Returns `true` when aborted.
+    fn abort_by_index(&mut self, idx: usize) -> bool {
+        self.canregs.tsr().write(|reg| reg.set_abrq(idx, true));
+
+        // Wait for the abort request to be finished.
+        loop {
+            let tsr = self.canregs.tsr().read();
+            if false == tsr.abrq(idx) {
+                break tsr.txok(idx) == false;
+            }
+        }
+    }
+
+    /// Attempts to abort the sending of a frame that is pending in a mailbox.
+    ///
+    /// If there is no frame in the provided mailbox, or its transmission succeeds before it can be
+    /// aborted, this function has no effect and returns `false`.
+    ///
+    /// If there is a frame in the provided mailbox, and it is canceled successfully, this function
+    /// returns `true`.
+    pub fn abort(&mut self, mailbox: Mailbox) -> bool {
+        // If the mailbox is empty, the value of TXOKx depends on what happened with the previous
+        // frame in that mailbox. Only call abort_by_index() if the mailbox is not empty.
+        let tsr = self.canregs.tsr().read();
+        let mailbox_empty = match mailbox {
+            Mailbox::Mailbox0 => tsr.tme(0),
+            Mailbox::Mailbox1 => tsr.tme(1),
+            Mailbox::Mailbox2 => tsr.tme(2),
+        };
+        if mailbox_empty {
+            false
+        } else {
+            self.abort_by_index(mailbox as usize)
+        }
+    }
+
+    /// Returns `true` if no frame is pending for transmission.
+    pub fn is_idle(&self) -> bool {
+        let tsr = self.canregs.tsr().read();
+        tsr.tme(0) && tsr.tme(1) && tsr.tme(2)
+    }
+
+    /// Clears the request complete flag for all mailboxes.
+    pub fn clear_interrupt_flags(&mut self) {
+        self.canregs.tsr().write(|reg| {
+            reg.set_rqcp(0, true);
+            reg.set_rqcp(1, true);
+            reg.set_rqcp(2, true);
+        });
+    }
+
+    pub fn receive_fifo(&self, fifo: crate::can::_version::bx::RxFifo) -> Option<Envelope> {
+        // Generate timestamp as early as possible
+        #[cfg(feature = "time")]
+        let ts = embassy_time::Instant::now();
+
+        use crate::pac::can::vals::Ide;
+
+        let fifo_idx = match fifo {
+            crate::can::_version::bx::RxFifo::Fifo0 => 0usize,
+            crate::can::_version::bx::RxFifo::Fifo1 => 1usize,
+        };
+        let rfr = self.canregs.rfr(fifo_idx);
+        let fifo = self.canregs.rx(fifo_idx);
+
+        // If there are no pending messages, there is nothing to do
+        if rfr.read().fmp() == 0 {
+            return None;
+        }
+
+        let rir = fifo.rir().read();
+        let id: embedded_can::Id = if rir.ide() == Ide::STANDARD {
+            embedded_can::StandardId::new(rir.stid()).unwrap().into()
+        } else {
+            let stid = (rir.stid() & 0x7FF) as u32;
+            let exid = rir.exid() & 0x3FFFF;
+            let id = (stid << 18) | (exid);
+            embedded_can::ExtendedId::new(id).unwrap().into()
+        };
+        let data_len = fifo.rdtr().read().dlc();
+        let mut data: [u8; 8] = [0; 8];
+        data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes());
+        data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes());
+
+        let frame = Frame::new(Header::new(id, data_len, false), &data).unwrap();
+        let envelope = Envelope {
+            #[cfg(feature = "time")]
+            ts,
+            frame,
+        };
+
+        rfr.modify(|v| v.set_rfom(true));
+
+        Some(envelope)
+    }
+}
+
 /// Configuration proxy returned by [`Can::modify_config`].
 #[must_use = "`CanConfig` leaves the peripheral in uninitialized state, call `CanConfig::enable` or explicitly drop the value"]
 pub struct CanConfig<'a, I: Instance> {
@@ -253,24 +651,20 @@ impl<I: Instance> CanConfig<'_, I> {
     /// Then copy the `CAN_BUS_TIME` register value from the table and pass it as the `btr`
     /// parameter to this method.
     pub fn set_bit_timing(self, bt: crate::can::util::NominalBitTiming) -> Self {
-        self.can.set_bit_timing(bt);
+        self.can.registers.set_bit_timing(bt);
         self
     }
 
     /// Enables or disables loopback mode: Internally connects the TX and RX
     /// signals together.
     pub fn set_loopback(self, enabled: bool) -> Self {
-        self.can.canregs.btr().modify(|reg| reg.set_lbkm(enabled));
+        self.can.registers.set_loopback(enabled);
         self
     }
 
     /// Enables or disables silent mode: Disconnects the TX signal from the pin.
     pub fn set_silent(self, enabled: bool) -> Self {
-        let mode = match enabled {
-            false => stm32_metapac::can::vals::Silm::NORMAL,
-            true => stm32_metapac::can::vals::Silm::SILENT,
-        };
-        self.can.canregs.btr().modify(|reg| reg.set_silm(mode));
+        self.can.registers.set_silent(enabled);
         self
     }
 
@@ -281,7 +675,7 @@ impl<I: Instance> CanConfig<'_, I> {
     ///
     /// Automatic retransmission is enabled by default.
     pub fn set_automatic_retransmit(self, enabled: bool) -> Self {
-        self.can.canregs.mcr().modify(|reg| reg.set_nart(enabled));
+        self.can.registers.set_automatic_retransmit(enabled);
         self
     }
 
@@ -292,10 +686,10 @@ impl<I: Instance> CanConfig<'_, I> {
     ///
     /// If you want to finish configuration without enabling the peripheral, you can call
     /// [`CanConfig::leave_disabled`] or [`drop`] the [`CanConfig`] instead.
-    pub fn enable(mut self) {
-        self.leave_init_mode();
+    pub fn enable(self) {
+        self.can.registers.leave_init_mode();
 
-        match nb::block!(self.can.enable_non_blocking()) {
+        match nb::block!(self.can.registers.enable_non_blocking()) {
             Ok(()) => {}
             Err(void) => match void {},
         }
@@ -308,29 +702,15 @@ impl<I: Instance> CanConfig<'_, I> {
     ///
     /// Before the [`Can`] instance can be used, you have to enable it by calling
     /// [`Can::enable_non_blocking`].
-    pub fn leave_disabled(mut self) {
-        self.leave_init_mode();
-    }
-
-    /// Leaves initialization mode, enters sleep mode.
-    fn leave_init_mode(&mut self) {
-        self.can.canregs.mcr().modify(|reg| {
-            reg.set_sleep(true);
-            reg.set_inrq(false);
-        });
-        loop {
-            let msr = self.can.canregs.msr().read();
-            if msr.slak() && !msr.inak() {
-                break;
-            }
-        }
+    pub fn leave_disabled(self) {
+        self.can.registers.leave_init_mode();
     }
 }
 
 impl<I: Instance> Drop for CanConfig<'_, I> {
     #[inline]
     fn drop(&mut self) {
-        self.leave_init_mode();
+        self.can.registers.leave_init_mode();
     }
 }
 
@@ -354,23 +734,19 @@ impl<I: Instance> CanBuilder<I> {
     /// Then copy the `CAN_BUS_TIME` register value from the table and pass it as the `btr`
     /// parameter to this method.
     pub fn set_bit_timing(mut self, bt: crate::can::util::NominalBitTiming) -> Self {
-        self.can.set_bit_timing(bt);
+        self.can.registers.set_bit_timing(bt);
         self
     }
     /// Enables or disables loopback mode: Internally connects the TX and RX
     /// signals together.
     pub fn set_loopback(self, enabled: bool) -> Self {
-        self.can.canregs.btr().modify(|reg| reg.set_lbkm(enabled));
+        self.can.registers.set_loopback(enabled);
         self
     }
 
     /// Enables or disables silent mode: Disconnects the TX signal from the pin.
     pub fn set_silent(self, enabled: bool) -> Self {
-        let mode = match enabled {
-            false => stm32_metapac::can::vals::Silm::NORMAL,
-            true => stm32_metapac::can::vals::Silm::SILENT,
-        };
-        self.can.canregs.btr().modify(|reg| reg.set_silm(mode));
+        self.can.registers.set_silent(enabled);
         self
     }
 
@@ -381,7 +757,7 @@ impl<I: Instance> CanBuilder<I> {
     ///
     /// Automatic retransmission is enabled by default.
     pub fn set_automatic_retransmit(self, enabled: bool) -> Self {
-        self.can.canregs.mcr().modify(|reg| reg.set_nart(enabled));
+        self.can.registers.set_automatic_retransmit(enabled);
         self
     }
 
@@ -395,7 +771,7 @@ impl<I: Instance> CanBuilder<I> {
     pub fn enable(mut self) -> Can<I> {
         self.leave_init_mode();
 
-        match nb::block!(self.can.enable_non_blocking()) {
+        match nb::block!(self.can.registers.enable_non_blocking()) {
             Ok(()) => self.can,
             Err(void) => match void {},
         }
@@ -415,16 +791,7 @@ impl<I: Instance> CanBuilder<I> {
 
     /// Leaves initialization mode, enters sleep mode.
     fn leave_init_mode(&mut self) {
-        self.can.canregs.mcr().modify(|reg| {
-            reg.set_sleep(true);
-            reg.set_inrq(false);
-        });
-        loop {
-            let msr = self.can.canregs.msr().read();
-            if msr.slak() && !msr.inak() {
-                break;
-            }
-        }
+        self.can.registers.leave_init_mode();
     }
 }
 
@@ -432,6 +799,7 @@ impl<I: Instance> CanBuilder<I> {
 pub struct Can<I: Instance> {
     instance: I,
     canregs: crate::pac::can::Can,
+    pub(crate) registers: Registers,
 }
 
 impl<I> Can<I>
@@ -440,50 +808,25 @@ where
 {
     /// Creates a [`CanBuilder`] for constructing a CAN interface.
     pub fn builder(instance: I, canregs: crate::pac::can::Can) -> CanBuilder<I> {
-        let can_builder = CanBuilder {
-            can: Can { instance, canregs },
+        let mut can_builder = CanBuilder {
+            can: Can {
+                instance,
+                canregs,
+                registers: Registers { canregs },
+            },
         };
 
-        canregs.mcr().modify(|reg| {
-            reg.set_sleep(false);
-            reg.set_inrq(true);
-        });
-        loop {
-            let msr = canregs.msr().read();
-            if !msr.slak() && msr.inak() {
-                break;
-            }
-        }
+        can_builder.can.registers.enter_init_mode();
 
         can_builder
     }
 
-    fn set_bit_timing(&mut self, bt: crate::can::util::NominalBitTiming) {
-        let prescaler = u16::from(bt.prescaler) & 0x1FF;
-        let seg1 = u8::from(bt.seg1);
-        let seg2 = u8::from(bt.seg2) & 0x7F;
-        let sync_jump_width = u8::from(bt.sync_jump_width) & 0x7F;
-        self.canregs.btr().modify(|reg| {
-            reg.set_brp(prescaler - 1);
-            reg.set_ts(0, seg1 - 1);
-            reg.set_ts(1, seg2 - 1);
-            reg.set_sjw(sync_jump_width - 1);
-        });
-    }
-
-    /// Returns a reference to the peripheral instance.
-    ///
-    /// This allows accessing HAL-specific data stored in the instance type.
-    pub fn instance(&mut self) -> &mut I {
-        &mut self.instance
-    }
-
     /// Disables the CAN interface and returns back the raw peripheral it was created from.
     ///
     /// The peripheral is disabled by setting `RESET` in `CAN_MCR`, which causes the peripheral to
     /// enter sleep mode.
     pub fn free(self) -> I {
-        self.canregs.mcr().write(|reg| reg.set_reset(true));
+        self.registers.reset();
         self.instance
     }
 
@@ -491,85 +834,11 @@ where
     ///
     /// Calling this method will enter initialization mode.
     pub fn modify_config(&mut self) -> CanConfig<'_, I> {
-        self.canregs.mcr().modify(|reg| {
-            reg.set_sleep(false);
-            reg.set_inrq(true);
-        });
-        loop {
-            let msr = self.canregs.msr().read();
-            if !msr.slak() && msr.inak() {
-                break;
-            }
-        }
+        self.registers.enter_init_mode();
 
         CanConfig { can: self }
     }
 
-    /// Configures the automatic wake-up feature.
-    ///
-    /// This is turned off by default.
-    ///
-    /// When turned on, an incoming frame will cause the peripheral to wake up from sleep and
-    /// receive the frame. If enabled, [`Interrupt::Wakeup`] will also be triggered by the incoming
-    /// frame.
-    pub fn set_automatic_wakeup(&mut self, enabled: bool) {
-        self.canregs.mcr().modify(|reg| reg.set_awum(enabled));
-    }
-
-    /// Leaves initialization mode and enables the peripheral (non-blocking version).
-    ///
-    /// Usually, it is recommended to call [`CanConfig::enable`] instead. This method is only needed
-    /// if you want non-blocking initialization.
-    ///
-    /// If this returns [`WouldBlock`][nb::Error::WouldBlock], the peripheral will enable itself
-    /// in the background. The peripheral is enabled and ready to use when this method returns
-    /// successfully.
-    pub fn enable_non_blocking(&mut self) -> nb::Result<(), Infallible> {
-        let msr = self.canregs.msr().read();
-        if msr.slak() {
-            self.canregs.mcr().modify(|reg| {
-                reg.set_abom(true);
-                reg.set_sleep(false);
-            });
-            Err(nb::Error::WouldBlock)
-        } else {
-            Ok(())
-        }
-    }
-
-    /// Puts the peripheral in a sleep mode to save power.
-    ///
-    /// While in sleep mode, an incoming CAN frame will trigger [`Interrupt::Wakeup`] if enabled.
-    pub fn sleep(&mut self) {
-        self.canregs.mcr().modify(|reg| {
-            reg.set_sleep(true);
-            reg.set_inrq(false);
-        });
-        loop {
-            let msr = self.canregs.msr().read();
-            if msr.slak() && !msr.inak() {
-                break;
-            }
-        }
-    }
-
-    /// Wakes up from sleep mode.
-    ///
-    /// Note that this will not trigger [`Interrupt::Wakeup`], only reception of an incoming CAN
-    /// frame will cause that interrupt.
-    pub fn wakeup(&mut self) {
-        self.canregs.mcr().modify(|reg| {
-            reg.set_sleep(false);
-            reg.set_inrq(false);
-        });
-        loop {
-            let msr = self.canregs.msr().read();
-            if !msr.slak() && !msr.inak() {
-                break;
-            }
-        }
-    }
-
     /// Puts a CAN frame in a free transmit mailbox for transmission on the bus.
     ///
     /// Frames are transmitted to the bus based on their priority (see [`FramePriority`]).
@@ -602,14 +871,12 @@ where
         unsafe { Tx::<I>::conjure(self.canregs).abort(mailbox) }
     }
 
-
     pub(crate) fn split_by_ref(&mut self) -> (Tx<I>, Rx<I>) {
         // Safety: We take `&mut self` and the return value lifetimes are tied to `self`'s lifetime.
         let tx = unsafe { Tx::conjure(self.canregs) };
         let rx0 = unsafe { Rx::conjure() };
         (tx, rx0)
     }
-
 }
 
 impl<I: FilterOwner> Can<I> {
@@ -625,7 +892,7 @@ impl<I: FilterOwner> Can<I> {
 /// Marker for Tx half
 pub struct Tx<I> {
     _can: PhantomData<I>,
-    canregs: crate::pac::can::Can,
+    pub(crate) registers: Registers,
 }
 
 impl<I> Tx<I>
@@ -635,7 +902,7 @@ where
     unsafe fn conjure(canregs: crate::pac::can::Can) -> Self {
         Self {
             _can: PhantomData,
-            canregs,
+            registers: Registers { canregs }, //canregs,
         }
     }
 
@@ -649,120 +916,7 @@ where
     /// cancelled and `frame` is enqueued instead. The frame that was replaced is returned as
     /// [`TransmitStatus::dequeued_frame`].
     pub fn transmit(&mut self, frame: &Frame) -> nb::Result<TransmitStatus, Infallible> {
-        // Get the index of the next free mailbox or the one with the lowest priority.
-        let tsr = self.canregs.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.
-            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)
-            } else {
-                // There was a free mailbox.
-                None
-            }
-        } else {
-            // All mailboxes are available: Send frame without performing any checks.
-            None
-        };
-
-        self.write_mailbox(idx, frame);
-
-        let mailbox = match idx {
-            0 => Mailbox::Mailbox0,
-            1 => Mailbox::Mailbox1,
-            2 => Mailbox::Mailbox2,
-            _ => unreachable!(),
-        };
-        Ok(TransmitStatus {
-            dequeued_frame: pending_frame,
-            mailbox,
-        })
-    }
-
-    /// Returns `Ok` when the mailbox is free or if it contains pending frame with a
-    /// lower priority (higher ID) than 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.canregs.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.
-            return Err(nb::Error::WouldBlock);
-        }
-
-        Ok(())
-    }
-
-    fn write_mailbox(&mut self, idx: usize, frame: &Frame) {
-        debug_assert!(idx < 3);
-
-        let mb = self.canregs.tx(idx);
-        mb.tdtr().write(|w| w.set_dlc(frame.header().len() as u8));
-
-        mb.tdlr()
-            .write(|w| w.0 = u32::from_ne_bytes(frame.data()[0..4].try_into().unwrap()));
-        mb.tdhr()
-            .write(|w| w.0 = u32::from_ne_bytes(frame.data()[4..8].try_into().unwrap()));
-        let id: IdReg = frame.id().into();
-        mb.tir().write(|w| {
-            w.0 = id.0;
-            w.set_txrq(true);
-        });
-    }
-
-    fn read_pending_mailbox(&mut self, idx: usize) -> Option<Frame> {
-        if self.abort_by_index(idx) {
-            debug_assert!(idx < 3);
-
-            let mb = self.canregs.tx(idx);
-
-            let id = IdReg(mb.tir().read().0);
-            let mut data = [0xff; 8];
-            data[0..4].copy_from_slice(&mb.tdlr().read().0.to_ne_bytes());
-            data[4..8].copy_from_slice(&mb.tdhr().read().0.to_ne_bytes());
-            let len = mb.tdtr().read().dlc();
-
-            Some(Frame::new(Header::new(id.id(), len, id.rtr()), &data).unwrap())
-        } 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
-        }
-    }
-
-    /// Tries to abort a pending frame. Returns `true` when aborted.
-    fn abort_by_index(&mut self, idx: usize) -> bool {
-        self.canregs.tsr().write(|reg| reg.set_abrq(idx, true));
-
-        // Wait for the abort request to be finished.
-        loop {
-            let tsr = self.canregs.tsr().read();
-            if false == tsr.abrq(idx) {
-                break tsr.txok(idx) == false;
-            }
-        }
+        self.registers.transmit(frame)
     }
 
     /// Attempts to abort the sending of a frame that is pending in a mailbox.
@@ -773,34 +927,17 @@ where
     /// If there is a frame in the provided mailbox, and it is canceled successfully, this function
     /// returns `true`.
     pub fn abort(&mut self, mailbox: Mailbox) -> bool {
-        // If the mailbox is empty, the value of TXOKx depends on what happened with the previous
-        // frame in that mailbox. Only call abort_by_index() if the mailbox is not empty.
-        let tsr = self.canregs.tsr().read();
-        let mailbox_empty = match mailbox {
-            Mailbox::Mailbox0 => tsr.tme(0),
-            Mailbox::Mailbox1 => tsr.tme(1),
-            Mailbox::Mailbox2 => tsr.tme(2),
-        };
-        if mailbox_empty {
-            false
-        } else {
-            self.abort_by_index(mailbox as usize)
-        }
+        self.registers.abort(mailbox)
     }
 
     /// Returns `true` if no frame is pending for transmission.
     pub fn is_idle(&self) -> bool {
-        let tsr = self.canregs.tsr().read();
-        tsr.tme(0) && tsr.tme(1) && tsr.tme(2)
+        self.registers.is_idle()
     }
 
     /// Clears the request complete flag for all mailboxes.
     pub fn clear_interrupt_flags(&mut self) {
-        self.canregs.tsr().write(|reg| {
-            reg.set_rqcp(0, true);
-            reg.set_rqcp(1, true);
-            reg.set_rqcp(2, true);
-        });
+        self.registers.clear_interrupt_flags()
     }
 }
 
@@ -814,9 +951,7 @@ where
     I: Instance,
 {
     unsafe fn conjure() -> Self {
-        Self {
-            _can: PhantomData,
-        }
+        Self { _can: PhantomData }
     }
 }
 
diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs
index 9d2b8797e..d865bbe7d 100644
--- a/embassy-stm32/src/can/bxcan.rs
+++ b/embassy-stm32/src/can/bxcan.rs
@@ -14,7 +14,6 @@ use futures::FutureExt;
 
 use crate::gpio::AFType;
 use crate::interrupt::typelevel::Interrupt;
-use crate::pac::can::vals::{Ide, Lec};
 use crate::rcc::RccPeripheral;
 use crate::{interrupt, peripherals, Peripheral};
 
@@ -185,7 +184,7 @@ impl<'d, T: Instance> Can<'d, T> {
     /// This will wait for 11 consecutive recessive bits (bus idle state).
     /// Contrary to enable method from bxcan library, this will not freeze the executor while waiting.
     pub async fn enable(&mut self) {
-        while self.enable_non_blocking().is_err() {
+        while self.registers.enable_non_blocking().is_err() {
             // SCE interrupt is only generated for entering sleep mode, but not leaving.
             // Yield to allow other tasks to execute while can bus is initializing.
             embassy_futures::yield_now().await;
@@ -243,52 +242,17 @@ impl<'d, T: Instance> Can<'d, T> {
     }
 
     unsafe fn receive_fifo(fifo: RxFifo) {
-        // Generate timestamp as early as possible
-        #[cfg(feature = "time")]
-        let ts = embassy_time::Instant::now();
-
         let state = T::state();
-        let regs = T::regs();
-        let fifo_idx = match fifo {
-            RxFifo::Fifo0 => 0usize,
-            RxFifo::Fifo1 => 1usize,
-        };
-        let rfr = regs.rfr(fifo_idx);
-        let fifo = regs.rx(fifo_idx);
+        let regsisters = crate::can::bx::Registers { canregs: T::regs() };
 
         loop {
-            // If there are no pending messages, there is nothing to do
-            if rfr.read().fmp() == 0 {
-                return;
-            }
-
-            let rir = fifo.rir().read();
-            let id: embedded_can::Id = if rir.ide() == Ide::STANDARD {
-                embedded_can::StandardId::new(rir.stid()).unwrap().into()
-            } else {
-                let stid = (rir.stid() & 0x7FF) as u32;
-                let exid = rir.exid() & 0x3FFFF;
-                let id = (stid << 18) | (exid);
-                embedded_can::ExtendedId::new(id).unwrap().into()
-            };
-            let data_len = fifo.rdtr().read().dlc();
-            let mut data: [u8; 8] = [0; 8];
-            data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes());
-            data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes());
-
-            let frame = Frame::new(Header::new(id, data_len, false), &data).unwrap();
-            let envelope = Envelope {
-                #[cfg(feature = "time")]
-                ts,
-                frame,
+            match regsisters.receive_fifo(fifo) {
+                Some(envelope) => {
+                    // NOTE: consensus was reached that if rx_queue is full, packets should be dropped
+                    let _ = state.rx_queue.try_send(envelope);
+                }
+                None => return,
             };
-
-            rfr.modify(|v| v.set_rfom(true));
-
-            /*
-                NOTE: consensus was reached that if rx_queue is full, packets should be dropped
-            */
-            let _ = state.rx_queue.try_send(envelope);
         }
     }
 
@@ -297,7 +261,7 @@ impl<'d, T: Instance> Can<'d, T> {
     /// Useful for doing separate transmit/receive tasks.
     pub fn split<'c>(&'c mut self) -> (CanTx<'d, T>, CanRx<'d, T>) {
         let (tx, rx) = self.can.split_by_ref();
-        (CanTx { tx }, CanRx { rx})
+        (CanTx { tx }, CanRx { rx })
     }
 }
 
@@ -459,10 +423,7 @@ impl<'d, T: Instance> CanRx<'d, T> {
     }
 }
 
-enum RxFifo {
-    Fifo0,
-    Fifo1,
-}
+use crate::can::bx::RxFifo;
 
 impl<'d, T: Instance> Drop for Can<'d, T> {
     fn drop(&mut self) {
@@ -601,21 +562,4 @@ impl Index for crate::can::bx::Mailbox {
     }
 }
 
-trait IntoBusError {
-    fn into_bus_err(self) -> Option<BusError>;
-}
 
-impl IntoBusError for Lec {
-    fn into_bus_err(self) -> Option<BusError> {
-        match self {
-            Lec::STUFF => Some(BusError::Stuff),
-            Lec::FORM => Some(BusError::Form),
-            Lec::ACK => Some(BusError::Acknowledge),
-            Lec::BITRECESSIVE => Some(BusError::BitRecessive),
-            Lec::BITDOMINANT => Some(BusError::BitDominant),
-            Lec::CRC => Some(BusError::Crc),
-            Lec::CUSTOM => Some(BusError::Software),
-            _ => None,
-        }
-    }
-}
diff --git a/embassy-stm32/src/can/common.rs b/embassy-stm32/src/can/common.rs
index 1de54e5a1..8c9990798 100644
--- a/embassy-stm32/src/can/common.rs
+++ b/embassy-stm32/src/can/common.rs
@@ -1,8 +1,15 @@
 use embassy_sync::channel::{DynamicReceiver, DynamicSender};
 
-use crate::can::_version::frame::*;
-use crate::can::_version::Timestamp;
 use crate::can::_version::enums::*;
+use crate::can::_version::frame::*;
+
+/// Timestamp for incoming packets. Use Embassy time when enabled.
+#[cfg(feature = "time")]
+pub type Timestamp = embassy_time::Instant;
+
+/// Timestamp for incoming packets.
+#[cfg(not(feature = "time"))]
+pub type Timestamp = u16;
 
 pub(crate) struct ClassicBufferedRxInner {
     pub rx_sender: DynamicSender<'static, Result<(ClassicFrame, Timestamp), BusError>>,
@@ -48,4 +55,3 @@ impl BufferedCanSender {
 /// Receiver that can be used for receiving CAN frames. Note, each CAN frame will only be received by one receiver.
 pub type BufferedCanReceiver =
     embassy_sync::channel::DynamicReceiver<'static, Result<(ClassicFrame, Timestamp), BusError>>;
-
diff --git a/embassy-stm32/src/can/fdcan.rs b/embassy-stm32/src/can/fdcan.rs
index c42c42853..42c9bd9f6 100644
--- a/embassy-stm32/src/can/fdcan.rs
+++ b/embassy-stm32/src/can/fdcan.rs
@@ -25,7 +25,8 @@ use fd::config::*;
 use fd::filter::*;
 pub use fd::{config, filter};
 use frame::*;
-pub use self::common::{BufferedCanSender, BufferedCanReceiver};
+
+pub use self::common::{BufferedCanReceiver, BufferedCanSender};
 
 /// Timestamp for incoming packets. Use Embassy time when enabled.
 #[cfg(feature = "time")]
@@ -416,7 +417,6 @@ pub struct BufferedCan<'d, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_S
     rx_buf: &'static RxBuf<RX_BUF_SIZE>,
 }
 
-
 impl<'c, 'd, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize>
     BufferedCan<'d, T, TX_BUF_SIZE, RX_BUF_SIZE>
 {