Merge branch 'main' into stm32l0-reset-rtc

13 files changed, 3321 insertions, 0 deletions

diff --git a/embassy-stm32/src/can/fd/config.rs b/embassy-stm32/src/can/fd/config.rs
new file mode 100644
index 000000000..38b409121
--- /dev/null
+++ b/embassy-stm32/src/can/fd/config.rs
@@ -0,0 +1,438 @@
+//! Configuration for FDCAN Module
+//! Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+use core::num::{NonZeroU16, NonZeroU8};
+
+/// Configures the bit timings.
+///
+/// You can use <http://www.bittiming.can-wiki.info/> to calculate the `btr` parameter. Enter
+/// parameters as follows:
+///
+/// - *Clock Rate*: The input clock speed to the CAN peripheral (*not* the CPU clock speed).
+///   This is the clock rate of the peripheral bus the CAN peripheral is attached to (eg. APB1).
+/// - *Sample Point*: Should normally be left at the default value of 87.5%.
+/// - *SJW*: Should normally be left at the default value of 1.
+///
+/// Then copy the `CAN_BUS_TIME` register value from the table and pass it as the `btr`
+/// parameter to this method.
+#[derive(Clone, Copy, Debug)]
+pub struct NominalBitTiming {
+    /// Value by which the oscillator frequency is divided for generating the bit time quanta. The bit
+    /// time is built up from a multiple of this quanta. Valid values are 1 to 512.
+    pub prescaler: NonZeroU16,
+    /// Valid values are 1 to 128.
+    pub seg1: NonZeroU8,
+    /// Valid values are 1 to 255.
+    pub seg2: NonZeroU8,
+    /// Valid values are 1 to 128.
+    pub sync_jump_width: NonZeroU8,
+}
+impl NominalBitTiming {
+    #[inline]
+    pub(crate) fn nbrp(&self) -> u16 {
+        u16::from(self.prescaler) & 0x1FF
+    }
+    #[inline]
+    pub(crate) fn ntseg1(&self) -> u8 {
+        u8::from(self.seg1)
+    }
+    #[inline]
+    pub(crate) fn ntseg2(&self) -> u8 {
+        u8::from(self.seg2) & 0x7F
+    }
+    #[inline]
+    pub(crate) fn nsjw(&self) -> u8 {
+        u8::from(self.sync_jump_width) & 0x7F
+    }
+}
+
+impl Default for NominalBitTiming {
+    #[inline]
+    fn default() -> Self {
+        // Kernel Clock 8MHz, Bit rate: 500kbit/s. Corresponds to a NBTP
+        // register value of 0x0600_0A03
+        Self {
+            prescaler: NonZeroU16::new(1).unwrap(),
+            seg1: NonZeroU8::new(11).unwrap(),
+            seg2: NonZeroU8::new(4).unwrap(),
+            sync_jump_width: NonZeroU8::new(4).unwrap(),
+        }
+    }
+}
+
+/// Configures the data bit timings for the FdCan Variable Bitrates.
+/// This is not used when frame_transmit is set to anything other than AllowFdCanAndBRS.
+#[derive(Clone, Copy, Debug)]
+pub struct DataBitTiming {
+    /// Tranceiver Delay Compensation
+    pub transceiver_delay_compensation: bool,
+    ///  The value by which the oscillator frequency is divided to generate the bit time quanta. The bit
+    ///  time is built up from a multiple of this quanta. Valid values for the Baud Rate Prescaler are 1
+    ///  to 31.
+    pub prescaler: NonZeroU16,
+    /// Valid values are 1 to 31.
+    pub seg1: NonZeroU8,
+    /// Valid values are 1 to 15.
+    pub seg2: NonZeroU8,
+    /// Must always be smaller than DTSEG2, valid values are 1 to 15.
+    pub sync_jump_width: NonZeroU8,
+}
+impl DataBitTiming {
+    // #[inline]
+    // fn tdc(&self) -> u8 {
+    //     let tsd = self.transceiver_delay_compensation as u8;
+    //     //TODO: stm32g4 does not export the TDC field
+    //     todo!()
+    // }
+    #[inline]
+    pub(crate) fn dbrp(&self) -> u8 {
+        (u16::from(self.prescaler) & 0x001F) as u8
+    }
+    #[inline]
+    pub(crate) fn dtseg1(&self) -> u8 {
+        u8::from(self.seg1) & 0x1F
+    }
+    #[inline]
+    pub(crate) fn dtseg2(&self) -> u8 {
+        u8::from(self.seg2) & 0x0F
+    }
+    #[inline]
+    pub(crate) fn dsjw(&self) -> u8 {
+        u8::from(self.sync_jump_width) & 0x0F
+    }
+}
+
+impl Default for DataBitTiming {
+    #[inline]
+    fn default() -> Self {
+        // Kernel Clock 8MHz, Bit rate: 500kbit/s. Corresponds to a DBTP
+        // register value of 0x0000_0A33
+        Self {
+            transceiver_delay_compensation: false,
+            prescaler: NonZeroU16::new(1).unwrap(),
+            seg1: NonZeroU8::new(11).unwrap(),
+            seg2: NonZeroU8::new(4).unwrap(),
+            sync_jump_width: NonZeroU8::new(4).unwrap(),
+        }
+    }
+}
+
+/// Configures which modes to use
+/// Individual headers can contain a desire to be send via FdCan
+/// or use Bit rate switching. But if this general setting does not allow
+/// that, only classic CAN is used instead.
+#[derive(Clone, Copy, Debug)]
+pub enum FrameTransmissionConfig {
+    /// Only allow Classic CAN message Frames
+    ClassicCanOnly,
+    /// Allow (non-brs) FdCAN Message Frames
+    AllowFdCan,
+    /// Allow FdCAN Message Frames and allow Bit Rate Switching
+    AllowFdCanAndBRS,
+}
+
+///
+#[derive(Clone, Copy, Debug)]
+pub enum ClockDivider {
+    /// Divide by 1
+    _1 = 0b0000,
+    /// Divide by 2
+    _2 = 0b0001,
+    /// Divide by 4
+    _4 = 0b0010,
+    /// Divide by 6
+    _6 = 0b0011,
+    /// Divide by 8
+    _8 = 0b0100,
+    /// Divide by 10
+    _10 = 0b0101,
+    /// Divide by 12
+    _12 = 0b0110,
+    /// Divide by 14
+    _14 = 0b0111,
+    /// Divide by 16
+    _16 = 0b1000,
+    /// Divide by 18
+    _18 = 0b1001,
+    /// Divide by 20
+    _20 = 0b1010,
+    /// Divide by 22
+    _22 = 0b1011,
+    /// Divide by 24
+    _24 = 0b1100,
+    /// Divide by 26
+    _26 = 0b1101,
+    /// Divide by 28
+    _28 = 0b1110,
+    /// Divide by 30
+    _30 = 0b1111,
+}
+
+/// Prescaler of the Timestamp counter
+#[derive(Clone, Copy, Debug)]
+pub enum TimestampPrescaler {
+    /// 1
+    _1 = 1,
+    /// 2
+    _2 = 2,
+    /// 3
+    _3 = 3,
+    /// 4
+    _4 = 4,
+    /// 5
+    _5 = 5,
+    /// 6
+    _6 = 6,
+    /// 7
+    _7 = 7,
+    /// 8
+    _8 = 8,
+    /// 9
+    _9 = 9,
+    /// 10
+    _10 = 10,
+    /// 11
+    _11 = 11,
+    /// 12
+    _12 = 12,
+    /// 13
+    _13 = 13,
+    /// 14
+    _14 = 14,
+    /// 15
+    _15 = 15,
+    /// 16
+    _16 = 16,
+}
+
+/// Selects the source of the Timestamp counter
+#[derive(Clone, Copy, Debug)]
+pub enum TimestampSource {
+    /// The Timestamp counter is disabled
+    None,
+    /// Using the FdCan input clock as the Timstamp counter's source,
+    /// and using a specific prescaler
+    Prescaler(TimestampPrescaler),
+    /// Using TIM3 as a source
+    FromTIM3,
+}
+
+/// How to handle frames in the global filter
+#[derive(Clone, Copy, Debug)]
+pub enum NonMatchingFilter {
+    /// Frames will go to Fifo0 when they do no match any specific filter
+    IntoRxFifo0 = 0b00,
+    /// Frames will go to Fifo1 when they do no match any specific filter
+    IntoRxFifo1 = 0b01,
+    /// Frames will be rejected when they do not match any specific filter
+    Reject = 0b11,
+}
+
+/// How to handle frames which do not match a specific filter
+#[derive(Clone, Copy, Debug)]
+pub struct GlobalFilter {
+    /// How to handle non-matching standard frames
+    pub handle_standard_frames: NonMatchingFilter,
+
+    /// How to handle non-matching extended frames
+    pub handle_extended_frames: NonMatchingFilter,
+
+    /// How to handle remote standard frames
+    pub reject_remote_standard_frames: bool,
+
+    /// How to handle remote extended frames
+    pub reject_remote_extended_frames: bool,
+}
+impl GlobalFilter {
+    /// Reject all non-matching and remote frames
+    pub const fn reject_all() -> Self {
+        Self {
+            handle_standard_frames: NonMatchingFilter::Reject,
+            handle_extended_frames: NonMatchingFilter::Reject,
+            reject_remote_standard_frames: true,
+            reject_remote_extended_frames: true,
+        }
+    }
+
+    /// How to handle non-matching standard frames
+    pub const fn set_handle_standard_frames(mut self, filter: NonMatchingFilter) -> Self {
+        self.handle_standard_frames = filter;
+        self
+    }
+    /// How to handle non-matching exteded frames
+    pub const fn set_handle_extended_frames(mut self, filter: NonMatchingFilter) -> Self {
+        self.handle_extended_frames = filter;
+        self
+    }
+    /// How to handle remote standard frames
+    pub const fn set_reject_remote_standard_frames(mut self, filter: bool) -> Self {
+        self.reject_remote_standard_frames = filter;
+        self
+    }
+    /// How to handle remote extended frames
+    pub const fn set_reject_remote_extended_frames(mut self, filter: bool) -> Self {
+        self.reject_remote_extended_frames = filter;
+        self
+    }
+}
+impl Default for GlobalFilter {
+    #[inline]
+    fn default() -> Self {
+        Self {
+            handle_standard_frames: NonMatchingFilter::IntoRxFifo0,
+            handle_extended_frames: NonMatchingFilter::IntoRxFifo0,
+            reject_remote_standard_frames: false,
+            reject_remote_extended_frames: false,
+        }
+    }
+}
+
+/// FdCan Config Struct
+#[derive(Clone, Copy, Debug)]
+pub struct FdCanConfig {
+    /// Nominal Bit Timings
+    pub nbtr: NominalBitTiming,
+    /// (Variable) Data Bit Timings
+    pub dbtr: DataBitTiming,
+    /// Enables or disables automatic retransmission of messages
+    ///
+    /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame
+    /// util it can be sent. Otherwise, it will try only once to send each frame.
+    ///
+    /// Automatic retransmission is enabled by default.
+    pub automatic_retransmit: bool,
+    /// Enabled or disables the pausing between transmissions
+    ///
+    /// This feature looses up burst transmissions coming from a single node and it protects against
+    /// "babbling idiot" scenarios where the application program erroneously requests too many
+    /// transmissions.
+    pub transmit_pause: bool,
+    /// Enabled or disables the pausing between transmissions
+    ///
+    /// This feature looses up burst transmissions coming from a single node and it protects against
+    /// "babbling idiot" scenarios where the application program erroneously requests too many
+    /// transmissions.
+    pub frame_transmit: FrameTransmissionConfig,
+    /// Non Isoe Mode
+    /// If this is set, the FDCAN uses the CAN FD frame format as specified by the Bosch CAN
+    /// FD Specification V1.0.
+    pub non_iso_mode: bool,
+    /// Edge Filtering: Two consecutive dominant tq required to detect an edge for hard synchronization
+    pub edge_filtering: bool,
+    /// Enables protocol exception handling
+    pub protocol_exception_handling: bool,
+    /// Sets the general clock divider for this FdCAN instance
+    pub clock_divider: ClockDivider,
+    /// Sets the timestamp source
+    pub timestamp_source: TimestampSource,
+    /// Configures the Global Filter
+    pub global_filter: GlobalFilter,
+}
+
+impl FdCanConfig {
+    /// Configures the bit timings.
+    #[inline]
+    pub const fn set_nominal_bit_timing(mut self, btr: NominalBitTiming) -> Self {
+        self.nbtr = btr;
+        self
+    }
+
+    /// Configures the bit timings.
+    #[inline]
+    pub const fn set_data_bit_timing(mut self, btr: DataBitTiming) -> Self {
+        self.dbtr = btr;
+        self
+    }
+
+    /// Enables or disables automatic retransmission of messages
+    ///
+    /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame
+    /// util it can be sent. Otherwise, it will try only once to send each frame.
+    ///
+    /// Automatic retransmission is enabled by default.
+    #[inline]
+    pub const fn set_automatic_retransmit(mut self, enabled: bool) -> Self {
+        self.automatic_retransmit = enabled;
+        self
+    }
+
+    /// Enabled or disables the pausing between transmissions
+    ///
+    /// This feature looses up burst transmissions coming from a single node and it protects against
+    /// "babbling idiot" scenarios where the application program erroneously requests too many
+    /// transmissions.
+    #[inline]
+    pub const fn set_transmit_pause(mut self, enabled: bool) -> Self {
+        self.transmit_pause = enabled;
+        self
+    }
+
+    /// If this is set, the FDCAN uses the CAN FD frame format as specified by the Bosch CAN
+    /// FD Specification V1.0.
+    #[inline]
+    pub const fn set_non_iso_mode(mut self, enabled: bool) -> Self {
+        self.non_iso_mode = enabled;
+        self
+    }
+
+    /// Two consecutive dominant tq required to detect an edge for hard synchronization
+    #[inline]
+    pub const fn set_edge_filtering(mut self, enabled: bool) -> Self {
+        self.edge_filtering = enabled;
+        self
+    }
+
+    /// Sets the allowed transmission types for messages.
+    #[inline]
+    pub const fn set_frame_transmit(mut self, fts: FrameTransmissionConfig) -> Self {
+        self.frame_transmit = fts;
+        self
+    }
+
+    /// Enables protocol exception handling
+    #[inline]
+    pub const fn set_protocol_exception_handling(mut self, peh: bool) -> Self {
+        self.protocol_exception_handling = peh;
+        self
+    }
+
+    /// Sets the general clock divider for this FdCAN instance
+    #[inline]
+    pub const fn set_clock_divider(mut self, div: ClockDivider) -> Self {
+        self.clock_divider = div;
+        self
+    }
+
+    /// Sets the timestamp source
+    #[inline]
+    pub const fn set_timestamp_source(mut self, tss: TimestampSource) -> Self {
+        self.timestamp_source = tss;
+        self
+    }
+
+    /// Sets the global filter settings
+    #[inline]
+    pub const fn set_global_filter(mut self, filter: GlobalFilter) -> Self {
+        self.global_filter = filter;
+        self
+    }
+}
+
+impl Default for FdCanConfig {
+    #[inline]
+    fn default() -> Self {
+        Self {
+            nbtr: NominalBitTiming::default(),
+            dbtr: DataBitTiming::default(),
+            automatic_retransmit: true,
+            transmit_pause: false,
+            frame_transmit: FrameTransmissionConfig::ClassicCanOnly,
+            non_iso_mode: false,
+            edge_filtering: false,
+            protocol_exception_handling: true,
+            clock_divider: ClockDivider::_1,
+            timestamp_source: TimestampSource::None,
+            global_filter: GlobalFilter::default(),
+        }
+    }
+}
diff --git a/embassy-stm32/src/can/fd/filter.rs b/embassy-stm32/src/can/fd/filter.rs
new file mode 100644
index 000000000..3e2129e6e
--- /dev/null
+++ b/embassy-stm32/src/can/fd/filter.rs
@@ -0,0 +1,379 @@
+//! Definition of Filter structs for FDCAN Module
+//! Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+use embedded_can::{ExtendedId, StandardId};
+
+use crate::can::fd::message_ram;
+pub use crate::can::fd::message_ram::{EXTENDED_FILTER_MAX, STANDARD_FILTER_MAX};
+
+/// A Standard Filter
+pub type StandardFilter = Filter<StandardId, u16>;
+/// An Extended Filter
+pub type ExtendedFilter = Filter<ExtendedId, u32>;
+
+impl Default for StandardFilter {
+    fn default() -> Self {
+        StandardFilter::disable()
+    }
+}
+impl Default for ExtendedFilter {
+    fn default() -> Self {
+        ExtendedFilter::disable()
+    }
+}
+
+impl StandardFilter {
+    /// Accept all messages in FIFO 0
+    pub fn accept_all_into_fifo0() -> StandardFilter {
+        StandardFilter {
+            filter: FilterType::BitMask { filter: 0x0, mask: 0x0 },
+            action: Action::StoreInFifo0,
+        }
+    }
+
+    /// Accept all messages in FIFO 1
+    pub fn accept_all_into_fifo1() -> StandardFilter {
+        StandardFilter {
+            filter: FilterType::BitMask { filter: 0x0, mask: 0x0 },
+            action: Action::StoreInFifo1,
+        }
+    }
+
+    /// Reject all messages
+    pub fn reject_all() -> StandardFilter {
+        StandardFilter {
+            filter: FilterType::BitMask { filter: 0x0, mask: 0x0 },
+            action: Action::Reject,
+        }
+    }
+
+    /// Disable the filter
+    pub fn disable() -> StandardFilter {
+        StandardFilter {
+            filter: FilterType::Disabled,
+            action: Action::Disable,
+        }
+    }
+}
+
+impl ExtendedFilter {
+    /// Accept all messages in FIFO 0
+    pub fn accept_all_into_fifo0() -> ExtendedFilter {
+        ExtendedFilter {
+            filter: FilterType::BitMask { filter: 0x0, mask: 0x0 },
+            action: Action::StoreInFifo0,
+        }
+    }
+
+    /// Accept all messages in FIFO 1
+    pub fn accept_all_into_fifo1() -> ExtendedFilter {
+        ExtendedFilter {
+            filter: FilterType::BitMask { filter: 0x0, mask: 0x0 },
+            action: Action::StoreInFifo1,
+        }
+    }
+
+    /// Reject all messages
+    pub fn reject_all() -> ExtendedFilter {
+        ExtendedFilter {
+            filter: FilterType::BitMask { filter: 0x0, mask: 0x0 },
+            action: Action::Reject,
+        }
+    }
+
+    /// Disable the filter
+    pub fn disable() -> ExtendedFilter {
+        ExtendedFilter {
+            filter: FilterType::Disabled,
+            action: Action::Disable,
+        }
+    }
+}
+
+/// Filter Type
+#[derive(Clone, Copy, Debug)]
+pub enum FilterType<ID, UNIT>
+where
+    ID: Copy + Clone + core::fmt::Debug,
+    UNIT: Copy + Clone + core::fmt::Debug,
+{
+    /// Match with a range between two messages
+    Range {
+        /// First Id of the range
+        from: ID,
+        /// Last Id of the range
+        to: ID,
+    },
+    /// Match with a bitmask
+    BitMask {
+        /// Filter of the bitmask
+        filter: UNIT,
+        /// Mask of the bitmask
+        mask: UNIT,
+    },
+    /// Match with a single ID
+    DedicatedSingle(ID),
+    /// Match with one of two ID's
+    DedicatedDual(ID, ID),
+    /// Filter is disabled
+    Disabled,
+}
+impl<ID, UNIT> From<FilterType<ID, UNIT>> for message_ram::enums::FilterType
+where
+    ID: Copy + Clone + core::fmt::Debug,
+    UNIT: Copy + Clone + core::fmt::Debug,
+{
+    fn from(f: FilterType<ID, UNIT>) -> Self {
+        match f {
+            FilterType::Range { to: _, from: _ } => Self::RangeFilter,
+            FilterType::BitMask { filter: _, mask: _ } => Self::ClassicFilter,
+            FilterType::DedicatedSingle(_) => Self::DualIdFilter,
+            FilterType::DedicatedDual(_, _) => Self::DualIdFilter,
+            FilterType::Disabled => Self::FilterDisabled,
+        }
+    }
+}
+
+/// Filter Action
+#[derive(Clone, Copy, Debug)]
+pub enum Action {
+    /// No Action
+    Disable = 0b000,
+    /// Store an matching message in FIFO 0
+    StoreInFifo0 = 0b001,
+    /// Store an matching message in FIFO 1
+    StoreInFifo1 = 0b010,
+    /// Reject an matching message
+    Reject = 0b011,
+    /// Flag a matching message (But not store?!?)
+    FlagHighPrio = 0b100,
+    /// Flag a matching message as a High Priority message and store it in FIFO 0
+    FlagHighPrioAndStoreInFifo0 = 0b101,
+    /// Flag a matching message as a High Priority message and store it in FIFO 1
+    FlagHighPrioAndStoreInFifo1 = 0b110,
+}
+impl From<Action> for message_ram::enums::FilterElementConfig {
+    fn from(a: Action) -> Self {
+        match a {
+            Action::Disable => Self::DisableFilterElement,
+            Action::StoreInFifo0 => Self::StoreInFifo0,
+            Action::StoreInFifo1 => Self::StoreInFifo1,
+            Action::Reject => Self::Reject,
+            Action::FlagHighPrio => Self::SetPriority,
+            Action::FlagHighPrioAndStoreInFifo0 => Self::SetPriorityAndStoreInFifo0,
+            Action::FlagHighPrioAndStoreInFifo1 => Self::SetPriorityAndStoreInFifo1,
+        }
+    }
+}
+
+/// Filter
+#[derive(Clone, Copy, Debug)]
+pub struct Filter<ID, UNIT>
+where
+    ID: Copy + Clone + core::fmt::Debug,
+    UNIT: Copy + Clone + core::fmt::Debug,
+{
+    /// How to match an incoming message
+    pub filter: FilterType<ID, UNIT>,
+    /// What to do with a matching message
+    pub action: Action,
+}
+
+/// Standard Filter Slot
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+pub enum StandardFilterSlot {
+    /// 0
+    _0 = 0,
+    /// 1
+    _1 = 1,
+    /// 2
+    _2 = 2,
+    /// 3
+    _3 = 3,
+    /// 4
+    _4 = 4,
+    /// 5
+    _5 = 5,
+    /// 6
+    _6 = 6,
+    /// 7
+    _7 = 7,
+    /// 8
+    _8 = 8,
+    /// 9
+    _9 = 9,
+    /// 10
+    _10 = 10,
+    /// 11
+    _11 = 11,
+    /// 12
+    _12 = 12,
+    /// 13
+    _13 = 13,
+    /// 14
+    _14 = 14,
+    /// 15
+    _15 = 15,
+    /// 16
+    _16 = 16,
+    /// 17
+    _17 = 17,
+    /// 18
+    _18 = 18,
+    /// 19
+    _19 = 19,
+    /// 20
+    _20 = 20,
+    /// 21
+    _21 = 21,
+    /// 22
+    _22 = 22,
+    /// 23
+    _23 = 23,
+    /// 24
+    _24 = 24,
+    /// 25
+    _25 = 25,
+    /// 26
+    _26 = 26,
+    /// 27
+    _27 = 27,
+}
+impl From<u8> for StandardFilterSlot {
+    fn from(u: u8) -> Self {
+        match u {
+            0 => StandardFilterSlot::_0,
+            1 => StandardFilterSlot::_1,
+            2 => StandardFilterSlot::_2,
+            3 => StandardFilterSlot::_3,
+            4 => StandardFilterSlot::_4,
+            5 => StandardFilterSlot::_5,
+            6 => StandardFilterSlot::_6,
+            7 => StandardFilterSlot::_7,
+            8 => StandardFilterSlot::_8,
+            9 => StandardFilterSlot::_9,
+            10 => StandardFilterSlot::_10,
+            11 => StandardFilterSlot::_11,
+            12 => StandardFilterSlot::_12,
+            13 => StandardFilterSlot::_13,
+            14 => StandardFilterSlot::_14,
+            15 => StandardFilterSlot::_15,
+            16 => StandardFilterSlot::_16,
+            17 => StandardFilterSlot::_17,
+            18 => StandardFilterSlot::_18,
+            19 => StandardFilterSlot::_19,
+            20 => StandardFilterSlot::_20,
+            21 => StandardFilterSlot::_21,
+            22 => StandardFilterSlot::_22,
+            23 => StandardFilterSlot::_23,
+            24 => StandardFilterSlot::_24,
+            25 => StandardFilterSlot::_25,
+            26 => StandardFilterSlot::_26,
+            27 => StandardFilterSlot::_27,
+            _ => panic!("Standard Filter Slot Too High!"),
+        }
+    }
+}
+
+/// Extended Filter Slot
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+pub enum ExtendedFilterSlot {
+    /// 0
+    _0 = 0,
+    /// 1
+    _1 = 1,
+    /// 2
+    _2 = 2,
+    /// 3
+    _3 = 3,
+    /// 4
+    _4 = 4,
+    /// 5
+    _5 = 5,
+    /// 6
+    _6 = 6,
+    /// 7
+    _7 = 7,
+}
+impl From<u8> for ExtendedFilterSlot {
+    fn from(u: u8) -> Self {
+        match u {
+            0 => ExtendedFilterSlot::_0,
+            1 => ExtendedFilterSlot::_1,
+            2 => ExtendedFilterSlot::_2,
+            3 => ExtendedFilterSlot::_3,
+            4 => ExtendedFilterSlot::_4,
+            5 => ExtendedFilterSlot::_5,
+            6 => ExtendedFilterSlot::_6,
+            7 => ExtendedFilterSlot::_7,
+            _ => panic!("Extended Filter Slot Too High!"), // Should be unreachable
+        }
+    }
+}
+
+/// Enum over both Standard and Extended Filter ID's
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+pub enum FilterId {
+    /// Standard Filter Slots
+    Standard(StandardFilterSlot),
+    /// Extended Filter Slots
+    Extended(ExtendedFilterSlot),
+}
+
+pub(crate) trait ActivateFilter<ID, UNIT>
+where
+    ID: Copy + Clone + core::fmt::Debug,
+    UNIT: Copy + Clone + core::fmt::Debug,
+{
+    fn activate(&mut self, f: Filter<ID, UNIT>);
+    // fn read(&self) -> Filter<ID, UNIT>;
+}
+
+impl ActivateFilter<StandardId, u16> for message_ram::StandardFilter {
+    fn activate(&mut self, f: Filter<StandardId, u16>) {
+        let sft = f.filter.into();
+
+        let (sfid1, sfid2) = match f.filter {
+            FilterType::Range { to, from } => (to.as_raw(), from.as_raw()),
+            FilterType::DedicatedSingle(id) => (id.as_raw(), id.as_raw()),
+            FilterType::DedicatedDual(id1, id2) => (id1.as_raw(), id2.as_raw()),
+            FilterType::BitMask { filter, mask } => (filter, mask),
+            FilterType::Disabled => (0x0, 0x0),
+        };
+        let sfec = f.action.into();
+        self.write(|w| {
+            unsafe { w.sfid1().bits(sfid1).sfid2().bits(sfid2) }
+                .sft()
+                .set_filter_type(sft)
+                .sfec()
+                .set_filter_element_config(sfec)
+        });
+    }
+    // fn read(&self) -> Filter<StandardId, u16> {
+    //     todo!()
+    // }
+}
+impl ActivateFilter<ExtendedId, u32> for message_ram::ExtendedFilter {
+    fn activate(&mut self, f: Filter<ExtendedId, u32>) {
+        let eft = f.filter.into();
+
+        let (efid1, efid2) = match f.filter {
+            FilterType::Range { to, from } => (to.as_raw(), from.as_raw()),
+            FilterType::DedicatedSingle(id) => (id.as_raw(), id.as_raw()),
+            FilterType::DedicatedDual(id1, id2) => (id1.as_raw(), id2.as_raw()),
+            FilterType::BitMask { filter, mask } => (filter, mask),
+            FilterType::Disabled => (0x0, 0x0),
+        };
+        let efec = f.action.into();
+        self.write(|w| {
+            unsafe { w.efid1().bits(efid1).efid2().bits(efid2) }
+                .eft()
+                .set_filter_type(eft)
+                .efec()
+                .set_filter_element_config(efec)
+        });
+    }
+    // fn read(&self) -> Filter<ExtendedId, u32> {
+    //     todo!()
+    // }
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/common.rs b/embassy-stm32/src/can/fd/message_ram/common.rs
new file mode 100644
index 000000000..108c1a428
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/common.rs
@@ -0,0 +1,134 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+#![allow(non_camel_case_types)]
+#![allow(non_snake_case)]
+#![allow(unused)]
+
+use super::enums::{
+    BitRateSwitching, ErrorStateIndicator, FilterElementConfig, FilterType, FrameFormat, IdType,
+    RemoteTransmissionRequest,
+};
+use super::generic;
+
+#[doc = "Reader of field `ID`"]
+pub type ID_R = generic::R<u32, u32>;
+
+#[doc = "Reader of field `RTR`"]
+pub type RTR_R = generic::R<bool, RemoteTransmissionRequest>;
+impl RTR_R {
+    pub fn rtr(&self) -> RemoteTransmissionRequest {
+        match self.bits {
+            false => RemoteTransmissionRequest::TransmitDataFrame,
+            true => RemoteTransmissionRequest::TransmitRemoteFrame,
+        }
+    }
+    pub fn is_transmit_remote_frame(&self) -> bool {
+        *self == RemoteTransmissionRequest::TransmitRemoteFrame
+    }
+    pub fn is_transmit_data_frame(&self) -> bool {
+        *self == RemoteTransmissionRequest::TransmitDataFrame
+    }
+}
+
+#[doc = "Reader of field `XTD`"]
+pub type XTD_R = generic::R<bool, IdType>;
+impl XTD_R {
+    pub fn id_type(&self) -> IdType {
+        match self.bits() {
+            false => IdType::StandardId,
+            true => IdType::ExtendedId,
+        }
+    }
+    pub fn is_standard_id(&self) -> bool {
+        *self == IdType::StandardId
+    }
+    pub fn is_exteded_id(&self) -> bool {
+        *self == IdType::ExtendedId
+    }
+}
+
+#[doc = "Reader of field `ESI`"]
+pub type ESI_R = generic::R<bool, ErrorStateIndicator>;
+impl ESI_R {
+    pub fn error_state(&self) -> ErrorStateIndicator {
+        match self.bits() {
+            false => ErrorStateIndicator::ErrorActive,
+            true => ErrorStateIndicator::ErrorPassive,
+        }
+    }
+    pub fn is_error_active(&self) -> bool {
+        *self == ErrorStateIndicator::ErrorActive
+    }
+    pub fn is_error_passive(&self) -> bool {
+        *self == ErrorStateIndicator::ErrorPassive
+    }
+}
+
+#[doc = "Reader of field `DLC`"]
+pub type DLC_R = generic::R<u8, u8>;
+
+#[doc = "Reader of field `BRS`"]
+pub type BRS_R = generic::R<bool, BitRateSwitching>;
+impl BRS_R {
+    pub fn bit_rate_switching(&self) -> BitRateSwitching {
+        match self.bits() {
+            true => BitRateSwitching::WithBRS,
+            false => BitRateSwitching::WithoutBRS,
+        }
+    }
+    pub fn is_with_brs(&self) -> bool {
+        *self == BitRateSwitching::WithBRS
+    }
+    pub fn is_without_brs(&self) -> bool {
+        *self == BitRateSwitching::WithoutBRS
+    }
+}
+
+#[doc = "Reader of field `FDF`"]
+pub type FDF_R = generic::R<bool, FrameFormat>;
+impl FDF_R {
+    pub fn frame_format(&self) -> FrameFormat {
+        match self.bits() {
+            false => FrameFormat::Classic,
+            true => FrameFormat::Fdcan,
+        }
+    }
+    pub fn is_classic_format(&self) -> bool {
+        *self == FrameFormat::Classic
+    }
+    pub fn is_fdcan_format(&self) -> bool {
+        *self == FrameFormat::Fdcan
+    }
+}
+
+#[doc = "Reader of field `(X|S)FT`"]
+pub type ESFT_R = generic::R<u8, FilterType>;
+impl ESFT_R {
+    #[doc = r"Gets the Filtertype"]
+    #[inline(always)]
+    pub fn to_filter_type(&self) -> FilterType {
+        match self.bits() {
+            0b00 => FilterType::RangeFilter,
+            0b01 => FilterType::DualIdFilter,
+            0b10 => FilterType::ClassicFilter,
+            0b11 => FilterType::FilterDisabled,
+            _ => unreachable!(),
+        }
+    }
+}
+
+#[doc = "Reader of field `(E|S)FEC`"]
+pub type ESFEC_R = generic::R<u8, FilterElementConfig>;
+impl ESFEC_R {
+    pub fn to_filter_element_config(&self) -> FilterElementConfig {
+        match self.bits() {
+            0b000 => FilterElementConfig::DisableFilterElement,
+            0b001 => FilterElementConfig::StoreInFifo0,
+            0b010 => FilterElementConfig::StoreInFifo1,
+            0b011 => FilterElementConfig::Reject,
+            0b100 => FilterElementConfig::SetPriority,
+            0b101 => FilterElementConfig::SetPriorityAndStoreInFifo0,
+            0b110 => FilterElementConfig::SetPriorityAndStoreInFifo1,
+            _ => unimplemented!(),
+        }
+    }
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/enums.rs b/embassy-stm32/src/can/fd/message_ram/enums.rs
new file mode 100644
index 000000000..0ec5e0f34
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/enums.rs
@@ -0,0 +1,233 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+/// Datalength is the message length generalised over
+/// the Standard (Classic) and FDCAN message types
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum DataLength {
+    Classic(u8),
+    Fdcan(u8),
+}
+impl DataLength {
+    /// Creates a DataLength type
+    ///
+    /// Uses the byte length and Type of frame as input
+    pub fn new(len: u8, ff: FrameFormat) -> DataLength {
+        match ff {
+            FrameFormat::Classic => match len {
+                0..=8 => DataLength::Classic(len),
+                _ => panic!("DataLength > 8"),
+            },
+            FrameFormat::Fdcan => match len {
+                0..=64 => DataLength::Fdcan(len),
+                _ => panic!("DataLength > 64"),
+            },
+        }
+    }
+    /// Specialised function to create classic frames
+    pub fn new_classic(len: u8) -> DataLength {
+        Self::new(len, FrameFormat::Classic)
+    }
+    /// Specialised function to create FDCAN frames
+    pub fn new_fdcan(len: u8) -> DataLength {
+        Self::new(len, FrameFormat::Fdcan)
+    }
+
+    /// returns the length in bytes
+    pub fn len(&self) -> u8 {
+        match self {
+            DataLength::Classic(l) | DataLength::Fdcan(l) => *l,
+        }
+    }
+
+    pub(crate) fn dlc(&self) -> u8 {
+        match self {
+            DataLength::Classic(l) => *l,
+            // See RM0433 Rev 7 Table 475. DLC coding
+            DataLength::Fdcan(l) => match l {
+                0..=8 => *l,
+                9..=12 => 9,
+                13..=16 => 10,
+                17..=20 => 11,
+                21..=24 => 12,
+                25..=32 => 13,
+                33..=48 => 14,
+                49..=64 => 15,
+                _ => panic!("DataLength > 64"),
+            },
+        }
+    }
+}
+impl From<DataLength> for FrameFormat {
+    fn from(dl: DataLength) -> FrameFormat {
+        match dl {
+            DataLength::Classic(_) => FrameFormat::Classic,
+            DataLength::Fdcan(_) => FrameFormat::Fdcan,
+        }
+    }
+}
+
+/// Wheter or not to generate an Tx Event
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum Event {
+    /// Do not generate an Tx Event
+    NoEvent,
+    /// Generate an Tx Event with a specified ID
+    Event(u8),
+}
+
+impl From<Event> for EventControl {
+    fn from(e: Event) -> Self {
+        match e {
+            Event::NoEvent => EventControl::DoNotStore,
+            Event::Event(_) => EventControl::Store,
+        }
+    }
+}
+
+impl From<Option<u8>> for Event {
+    fn from(mm: Option<u8>) -> Self {
+        match mm {
+            None => Event::NoEvent,
+            Some(mm) => Event::Event(mm),
+        }
+    }
+}
+
+impl From<Event> for Option<u8> {
+    fn from(e: Event) -> Option<u8> {
+        match e {
+            Event::NoEvent => None,
+            Event::Event(mm) => Some(mm),
+        }
+    }
+}
+
+/// TODO
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum ErrorStateIndicator {
+    /// TODO
+    ErrorActive = 0,
+    /// TODO
+    ErrorPassive = 1,
+}
+impl From<ErrorStateIndicator> for bool {
+    #[inline(always)]
+    fn from(e: ErrorStateIndicator) -> Self {
+        e as u8 != 0
+    }
+}
+
+/// Type of frame, standard (classic) or FdCAN
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum FrameFormat {
+    Classic = 0,
+    Fdcan = 1,
+}
+impl From<FrameFormat> for bool {
+    #[inline(always)]
+    fn from(e: FrameFormat) -> Self {
+        e as u8 != 0
+    }
+}
+
+/// Type of Id, Standard or Extended
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum IdType {
+    /// Standard ID
+    StandardId = 0,
+    /// Extended ID
+    ExtendedId = 1,
+}
+impl From<IdType> for bool {
+    #[inline(always)]
+    fn from(e: IdType) -> Self {
+        e as u8 != 0
+    }
+}
+
+/// Whether the frame contains data or requests data
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum RemoteTransmissionRequest {
+    /// Frame contains data
+    TransmitDataFrame = 0,
+    /// frame does not contain data
+    TransmitRemoteFrame = 1,
+}
+impl From<RemoteTransmissionRequest> for bool {
+    #[inline(always)]
+    fn from(e: RemoteTransmissionRequest) -> Self {
+        e as u8 != 0
+    }
+}
+
+/// Whether BitRateSwitching should be or was enabled
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum BitRateSwitching {
+    /// disable bit rate switching
+    WithoutBRS = 0,
+    /// enable bit rate switching
+    WithBRS = 1,
+}
+impl From<BitRateSwitching> for bool {
+    #[inline(always)]
+    fn from(e: BitRateSwitching) -> Self {
+        e as u8 != 0
+    }
+}
+
+/// Whether to store transmit Events
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum EventControl {
+    /// do not store an tx event
+    DoNotStore,
+    /// store transmit events
+    Store,
+}
+impl From<EventControl> for bool {
+    #[inline(always)]
+    fn from(e: EventControl) -> Self {
+        e as u8 != 0
+    }
+}
+
+/// If an received message matched any filters
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum FilterFrameMatch {
+    /// This did match filter <id>
+    DidMatch(u8),
+    /// This received frame did not match any specific filters
+    DidNotMatch,
+}
+
+/// Type of filter to be used
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum FilterType {
+    /// Filter uses the range between two id's
+    RangeFilter = 0b00,
+    /// The filter matches on two specific id's (or one ID checked twice)
+    DualIdFilter = 0b01,
+    /// Filter is using a bitmask
+    ClassicFilter = 0b10,
+    /// Filter is disabled
+    FilterDisabled = 0b11,
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub enum FilterElementConfig {
+    /// Filter is disabled
+    DisableFilterElement = 0b000,
+    /// Store a matching message in FIFO 0
+    StoreInFifo0 = 0b001,
+    /// Store a matching message in FIFO 1
+    StoreInFifo1 = 0b010,
+    /// Reject a matching message
+    Reject = 0b011,
+    /// Flag that a priority message has been received, *But do note store!*??
+    SetPriority = 0b100,
+    /// Flag and store message in FIFO 0
+    SetPriorityAndStoreInFifo0 = 0b101,
+    /// Flag and store message in FIFO 1
+    SetPriorityAndStoreInFifo1 = 0b110,
+    //_Unused = 0b111,
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/extended_filter.rs b/embassy-stm32/src/can/fd/message_ram/extended_filter.rs
new file mode 100644
index 000000000..453e9056e
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/extended_filter.rs
@@ -0,0 +1,136 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+#![allow(non_camel_case_types)]
+#![allow(non_snake_case)]
+#![allow(unused)]
+
+use super::common::{ESFEC_R, ESFT_R};
+use super::enums::{FilterElementConfig, FilterType};
+use super::generic;
+
+#[doc = "Reader of register ExtendedFilter"]
+pub(crate) type R = generic::R<super::ExtendedFilterType, super::ExtendedFilter>;
+#[doc = "Writer for register ExtendedFilter"]
+pub(crate) type W = generic::W<super::ExtendedFilterType, super::ExtendedFilter>;
+#[doc = "Register ExtendedFilter `reset()`'s"]
+impl generic::ResetValue for super::ExtendedFilter {
+    type Type = super::ExtendedFilterType;
+    #[inline(always)]
+    fn reset_value() -> Self::Type {
+        // Sets filter element to Disabled
+        [0x0, 0x0]
+    }
+}
+
+#[doc = "Reader of field `EFID2`"]
+pub(crate) type EFID2_R = generic::R<u32, u32>;
+#[doc = "Write proxy for field `EFID2`"]
+pub(crate) struct EFID2_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> EFID2_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u32) -> &'a mut W {
+        self.w.bits[1] = (self.w.bits[1] & !(0x1FFFFFFF)) | ((value as u32) & 0x1FFFFFFF);
+        self.w
+    }
+}
+
+#[doc = "Reader of field `EFID1`"]
+pub(crate) type EFID1_R = generic::R<u32, u32>;
+#[doc = "Write proxy for field `EFID1`"]
+pub(crate) struct EFID1_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> EFID1_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u32) -> &'a mut W {
+        self.w.bits[0] = (self.w.bits[0] & !(0x1FFFFFFF)) | ((value as u32) & 0x1FFFFFFF);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `EFEC`"]
+pub(crate) struct EFEC_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> EFEC_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u8) -> &'a mut W {
+        self.w.bits[0] = (self.w.bits[0] & !(0x07 << 29)) | (((value as u32) & 0x07) << 29);
+        self.w
+    }
+    #[doc = r"Sets the field according to FilterElementConfig"]
+    #[inline(always)]
+    pub fn set_filter_element_config(self, fec: FilterElementConfig) -> &'a mut W {
+        //SAFETY: FilterElementConfig only be valid options
+        unsafe { self.bits(fec as u8) }
+    }
+}
+
+#[doc = "Write proxy for field `EFT`"]
+pub(crate) struct EFT_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> EFT_W<'a> {
+    #[doc = r"Sets the field according the FilterType"]
+    #[inline(always)]
+    pub fn set_filter_type(self, filter: FilterType) -> &'a mut W {
+        //SAFETY: FilterType only be valid options
+        unsafe { self.bits(filter as u8) }
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u8) -> &'a mut W {
+        self.w.bits[1] = (self.w.bits[1] & !(0x03 << 30)) | (((value as u32) & 0x03) << 30);
+        self.w
+    }
+}
+
+impl R {
+    #[doc = "Byte 0 - Bits 0:28 - EFID1"]
+    #[inline(always)]
+    pub fn sfid1(&self) -> EFID1_R {
+        EFID1_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32)
+    }
+    #[doc = "Byte 0 - Bits 29:31 - EFEC"]
+    #[inline(always)]
+    pub fn efec(&self) -> ESFEC_R {
+        ESFEC_R::new(((self.bits[0] >> 29) & 0x07) as u8)
+    }
+    #[doc = "Byte 1 - Bits 0:28 - EFID2"]
+    #[inline(always)]
+    pub fn sfid2(&self) -> EFID2_R {
+        EFID2_R::new(((self.bits[1]) & 0x1FFFFFFF) as u32)
+    }
+    #[doc = "Byte 1 - Bits 30:31 - EFT"]
+    #[inline(always)]
+    pub fn eft(&self) -> ESFT_R {
+        ESFT_R::new(((self.bits[1] >> 30) & 0x03) as u8)
+    }
+}
+impl W {
+    #[doc = "Byte 0 - Bits 0:28 - EFID1"]
+    #[inline(always)]
+    pub fn efid1(&mut self) -> EFID1_W {
+        EFID1_W { w: self }
+    }
+    #[doc = "Byte 0 - Bits 29:31 - EFEC"]
+    #[inline(always)]
+    pub fn efec(&mut self) -> EFEC_W {
+        EFEC_W { w: self }
+    }
+    #[doc = "Byte 1 - Bits 0:28 - EFID2"]
+    #[inline(always)]
+    pub fn efid2(&mut self) -> EFID2_W {
+        EFID2_W { w: self }
+    }
+    #[doc = "Byte 1 - Bits 30:31 - EFT"]
+    #[inline(always)]
+    pub fn eft(&mut self) -> EFT_W {
+        EFT_W { w: self }
+    }
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/generic.rs b/embassy-stm32/src/can/fd/message_ram/generic.rs
new file mode 100644
index 000000000..1a5e121b4
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/generic.rs
@@ -0,0 +1,168 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+use core::marker;
+
+///This trait shows that register has `read` method
+///
+///Registers marked with `Writable` can be also `modify`'ed
+pub trait Readable {}
+
+///This trait shows that register has `write`, `write_with_zero` and `reset` method
+///
+///Registers marked with `Readable` can be also `modify`'ed
+pub trait Writable {}
+
+///Reset value of the register
+///
+///This value is initial value for `write` method.
+///It can be also directly writed to register by `reset` method.
+pub trait ResetValue {
+    ///Register size
+    type Type;
+    ///Reset value of the register
+    fn reset_value() -> Self::Type;
+}
+
+///This structure provides volatile access to register
+pub struct Reg<U, REG> {
+    register: vcell::VolatileCell<U>,
+    _marker: marker::PhantomData<REG>,
+}
+
+unsafe impl<U: Send, REG> Send for Reg<U, REG> {}
+
+impl<U, REG> Reg<U, REG>
+where
+    Self: Readable,
+    U: Copy,
+{
+    ///Reads the contents of `Readable` register
+    ///
+    ///You can read the contents of a register in such way:
+    ///```ignore
+    ///let bits = periph.reg.read().bits();
+    ///```
+    ///or get the content of a particular field of a register.
+    ///```ignore
+    ///let reader = periph.reg.read();
+    ///let bits = reader.field1().bits();
+    ///let flag = reader.field2().bit_is_set();
+    ///```
+    #[inline(always)]
+    pub fn read(&self) -> R<U, Self> {
+        R {
+            bits: self.register.get(),
+            _reg: marker::PhantomData,
+        }
+    }
+}
+
+impl<U, REG> Reg<U, REG>
+where
+    Self: ResetValue<Type = U> + Writable,
+    U: Copy,
+{
+    ///Writes the reset value to `Writable` register
+    ///
+    ///Resets the register to its initial state
+    #[inline(always)]
+    pub fn reset(&self) {
+        self.register.set(Self::reset_value())
+    }
+}
+
+impl<U, REG> Reg<U, REG>
+where
+    Self: ResetValue<Type = U> + Writable,
+    U: Copy,
+{
+    ///Writes bits to `Writable` register
+    ///
+    ///You can write raw bits into a register:
+    ///```ignore
+    ///periph.reg.write(|w| unsafe { w.bits(rawbits) });
+    ///```
+    ///or write only the fields you need:
+    ///```ignore
+    ///periph.reg.write(|w| w
+    ///    .field1().bits(newfield1bits)
+    ///    .field2().set_bit()
+    ///    .field3().variant(VARIANT)
+    ///);
+    ///```
+    ///Other fields will have reset value.
+    #[inline(always)]
+    pub fn write<F>(&self, f: F)
+    where
+        F: FnOnce(&mut W<U, Self>) -> &mut W<U, Self>,
+    {
+        self.register.set(
+            f(&mut W {
+                bits: Self::reset_value(),
+                _reg: marker::PhantomData,
+            })
+            .bits,
+        );
+    }
+}
+
+///Register/field reader
+///
+///Result of the [`read`](Reg::read) method of a register.
+///Also it can be used in the [`modify`](Reg::read) method
+pub struct R<U, T> {
+    pub(crate) bits: U,
+    _reg: marker::PhantomData<T>,
+}
+
+impl<U, T> R<U, T>
+where
+    U: Copy,
+{
+    ///Create new instance of reader
+    #[inline(always)]
+    pub(crate) fn new(bits: U) -> Self {
+        Self {
+            bits,
+            _reg: marker::PhantomData,
+        }
+    }
+    ///Read raw bits from register/field
+    #[inline(always)]
+    pub fn bits(&self) -> U {
+        self.bits
+    }
+}
+
+impl<U, T, FI> PartialEq<FI> for R<U, T>
+where
+    U: PartialEq,
+    FI: Copy + Into<U>,
+{
+    #[inline(always)]
+    fn eq(&self, other: &FI) -> bool {
+        self.bits.eq(&(*other).into())
+    }
+}
+
+impl<FI> R<bool, FI> {
+    ///Value of the field as raw bits
+    #[inline(always)]
+    pub fn bit(&self) -> bool {
+        self.bits
+    }
+    ///Returns `true` if the bit is clear (0)
+    #[inline(always)]
+    pub fn bit_is_clear(&self) -> bool {
+        !self.bit()
+    }
+}
+
+///Register writer
+///
+///Used as an argument to the closures in the [`write`](Reg::write) and [`modify`](Reg::modify) methods of the register
+pub struct W<U, REG> {
+    ///Writable bits
+    pub(crate) bits: U,
+    _reg: marker::PhantomData<REG>,
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/mod.rs b/embassy-stm32/src/can/fd/message_ram/mod.rs
new file mode 100644
index 000000000..830edf3bb
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/mod.rs
@@ -0,0 +1,170 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+use volatile_register::RW;
+
+pub(crate) mod common;
+pub(crate) mod enums;
+pub(crate) mod generic;
+
+/// Number of Receive Fifos configured by this module
+pub const RX_FIFOS_MAX: u8 = 2;
+/// Number of Receive Messages per RxFifo configured by this module
+pub const RX_FIFO_MAX: u8 = 3;
+/// Number of Transmit Messages configured by this module
+pub const TX_FIFO_MAX: u8 = 3;
+/// Number of Transmit Events configured by this module
+pub const TX_EVENT_MAX: u8 = 3;
+/// Number of Standard Filters configured by this module
+pub const STANDARD_FILTER_MAX: u8 = 28;
+/// Number of Extended Filters configured by this module
+pub const EXTENDED_FILTER_MAX: u8 = 8;
+
+/// MessageRam Overlay
+#[repr(C)]
+pub struct RegisterBlock {
+    pub(crate) filters: Filters,
+    pub(crate) receive: [Receive; RX_FIFOS_MAX as usize],
+    pub(crate) transmit: Transmit,
+}
+impl RegisterBlock {
+    pub fn reset(&mut self) {
+        self.filters.reset();
+        self.receive[0].reset();
+        self.receive[1].reset();
+        self.transmit.reset();
+    }
+}
+
+#[repr(C)]
+pub(crate) struct Filters {
+    pub(crate) flssa: [StandardFilter; STANDARD_FILTER_MAX as usize],
+    pub(crate) flesa: [ExtendedFilter; EXTENDED_FILTER_MAX as usize],
+}
+impl Filters {
+    pub fn reset(&mut self) {
+        for sf in &mut self.flssa {
+            sf.reset();
+        }
+        for ef in &mut self.flesa {
+            ef.reset();
+        }
+    }
+}
+
+#[repr(C)]
+pub(crate) struct Receive {
+    pub(crate) fxsa: [RxFifoElement; RX_FIFO_MAX as usize],
+}
+impl Receive {
+    pub fn reset(&mut self) {
+        for fe in &mut self.fxsa {
+            fe.reset();
+        }
+    }
+}
+
+#[repr(C)]
+pub(crate) struct Transmit {
+    pub(crate) efsa: [TxEventElement; TX_EVENT_MAX as usize],
+    pub(crate) tbsa: [TxBufferElement; TX_FIFO_MAX as usize],
+}
+impl Transmit {
+    pub fn reset(&mut self) {
+        for ee in &mut self.efsa {
+            ee.reset();
+        }
+        for be in &mut self.tbsa {
+            be.reset();
+        }
+    }
+}
+
+pub(crate) mod standard_filter;
+pub(crate) type StandardFilterType = u32;
+pub(crate) type StandardFilter = generic::Reg<StandardFilterType, _StandardFilter>;
+pub(crate) struct _StandardFilter;
+impl generic::Readable for StandardFilter {}
+impl generic::Writable for StandardFilter {}
+
+pub(crate) mod extended_filter;
+pub(crate) type ExtendedFilterType = [u32; 2];
+pub(crate) type ExtendedFilter = generic::Reg<ExtendedFilterType, _ExtendedFilter>;
+pub(crate) struct _ExtendedFilter;
+impl generic::Readable for ExtendedFilter {}
+impl generic::Writable for ExtendedFilter {}
+
+pub(crate) mod txevent_element;
+pub(crate) type TxEventElementType = [u32; 2];
+pub(crate) type TxEventElement = generic::Reg<TxEventElementType, _TxEventElement>;
+pub(crate) struct _TxEventElement;
+impl generic::Readable for TxEventElement {}
+impl generic::Writable for TxEventElement {}
+
+pub(crate) mod rxfifo_element;
+#[repr(C)]
+pub(crate) struct RxFifoElement {
+    pub(crate) header: RxFifoElementHeader,
+    pub(crate) data: [RW<u32>; 16],
+}
+impl RxFifoElement {
+    pub(crate) fn reset(&mut self) {
+        self.header.reset();
+        for byte in self.data.iter_mut() {
+            unsafe { byte.write(0) };
+        }
+    }
+}
+pub(crate) type RxFifoElementHeaderType = [u32; 2];
+pub(crate) type RxFifoElementHeader = generic::Reg<RxFifoElementHeaderType, _RxFifoElement>;
+pub(crate) struct _RxFifoElement;
+impl generic::Readable for RxFifoElementHeader {}
+impl generic::Writable for RxFifoElementHeader {}
+
+pub(crate) mod txbuffer_element;
+#[repr(C)]
+pub(crate) struct TxBufferElement {
+    pub(crate) header: TxBufferElementHeader,
+    pub(crate) data: [RW<u32>; 16],
+}
+impl TxBufferElement {
+    pub(crate) fn reset(&mut self) {
+        self.header.reset();
+        for byte in self.data.iter_mut() {
+            unsafe { byte.write(0) };
+        }
+    }
+}
+pub(crate) type TxBufferElementHeader = generic::Reg<TxBufferElementHeaderType, _TxBufferElement>;
+pub(crate) type TxBufferElementHeaderType = [u32; 2];
+pub(crate) struct _TxBufferElement;
+impl generic::Readable for TxBufferElementHeader {}
+impl generic::Writable for TxBufferElementHeader {}
+
+/// FdCan Message RAM instance.
+///
+/// # Safety
+///
+/// It is only safe to implement this trait, when:
+///
+/// * The implementing type has ownership of the Message RAM, preventing any
+///   other accesses to the register block.
+/// * `MSG_RAM` is a pointer to the Message RAM block and can be safely accessed
+/// for as long as ownership or a borrow of the implementing type is present.
+pub unsafe trait Instance {
+    const MSG_RAM: *mut RegisterBlock;
+    fn msg_ram(&self) -> &RegisterBlock {
+        unsafe { &*Self::MSG_RAM }
+    }
+    fn msg_ram_mut(&mut self) -> &mut RegisterBlock {
+        unsafe { &mut *Self::MSG_RAM }
+    }
+}
+
+// Ensure the RegisterBlock is the same size as on pg 1957 of RM0440.
+static_assertions::assert_eq_size!(Filters, [u32; 28 + 16]);
+static_assertions::assert_eq_size!(Receive, [u32; 54]);
+static_assertions::assert_eq_size!(Transmit, [u32; 6 + 54]);
+static_assertions::assert_eq_size!(
+    RegisterBlock,
+    [u32; 28 /*Standard Filters*/ +16 /*Extended Filters*/ +54 /*RxFifo0*/ +54 /*RxFifo1*/ +6 /*TxEvent*/ +54 /*TxFifo */]
+);
diff --git a/embassy-stm32/src/can/fd/message_ram/rxfifo_element.rs b/embassy-stm32/src/can/fd/message_ram/rxfifo_element.rs
new file mode 100644
index 000000000..48fc3a091
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/rxfifo_element.rs
@@ -0,0 +1,122 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+#![allow(non_camel_case_types)]
+#![allow(non_snake_case)]
+#![allow(unused)]
+
+use super::common::{BRS_R, DLC_R, ESI_R, FDF_R, ID_R, RTR_R, XTD_R};
+use super::enums::{DataLength, FilterFrameMatch, FrameFormat};
+use super::generic;
+
+#[doc = "Reader of register RxFifoElement"]
+pub(crate) type R = generic::R<super::RxFifoElementHeaderType, super::RxFifoElementHeader>;
+// #[doc = "Writer for register ExtendedFilter"]
+// pub(crate) type W = generic::W<super::RxFifoElementHeaderType, super::RxFifoElementHeader>;
+#[doc = "Register ExtendedFilter `reset()`'s"]
+impl generic::ResetValue for super::RxFifoElementHeader {
+    type Type = super::RxFifoElementHeaderType;
+    #[inline(always)]
+    fn reset_value() -> Self::Type {
+        [0x0, 0x0]
+    }
+}
+
+#[doc = "Reader of field `RXTS`"]
+pub(crate) type RXTS_R = generic::R<u16, u16>;
+
+#[doc = "Reader of field `FIDX`"]
+pub(crate) type FIDX_R = generic::R<u8, u8>;
+
+pub(crate) struct _ANMF;
+#[doc = "Reader of field `ANMF`"]
+pub(crate) type ANMF_R = generic::R<bool, _ANMF>;
+impl ANMF_R {
+    pub fn is_matching_frame(&self) -> bool {
+        self.bit_is_clear()
+    }
+}
+
+impl R {
+    #[doc = "Byte 0 - Bits 0:28 - ID"]
+    #[inline(always)]
+    pub fn id(&self) -> ID_R {
+        ID_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32)
+    }
+    #[doc = "Byte 0 - Bit 29 - RTR"]
+    #[inline(always)]
+    pub fn rtr(&self) -> RTR_R {
+        RTR_R::new(((self.bits[0] >> 29) & 0x01) != 0)
+    }
+    #[doc = "Byte 0 - Bit 30 - XTD"]
+    #[inline(always)]
+    pub fn xtd(&self) -> XTD_R {
+        XTD_R::new(((self.bits[0] >> 30) & 0x01) != 0)
+    }
+    #[doc = "Byte 0 - Bit 30 - ESI"]
+    #[inline(always)]
+    pub fn esi(&self) -> ESI_R {
+        ESI_R::new(((self.bits[0] >> 31) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 0:15 - RXTS"]
+    #[inline(always)]
+    pub fn txts(&self) -> RXTS_R {
+        RXTS_R::new(((self.bits[1]) & 0xFFFF) as u16)
+    }
+    #[doc = "Byte 1 - Bits 16:19 - DLC"]
+    #[inline(always)]
+    pub fn dlc(&self) -> DLC_R {
+        DLC_R::new(((self.bits[1] >> 16) & 0x0F) as u8)
+    }
+    #[doc = "Byte 1 - Bits 20 - BRS"]
+    #[inline(always)]
+    pub fn brs(&self) -> BRS_R {
+        BRS_R::new(((self.bits[1] >> 20) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 20 - FDF"]
+    #[inline(always)]
+    pub fn fdf(&self) -> FDF_R {
+        FDF_R::new(((self.bits[1] >> 21) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 24:30 - FIDX"]
+    #[inline(always)]
+    pub fn fidx(&self) -> FIDX_R {
+        FIDX_R::new(((self.bits[1] >> 24) & 0xFF) as u8)
+    }
+    #[doc = "Byte 1 - Bits 31 - ANMF"]
+    #[inline(always)]
+    pub fn anmf(&self) -> ANMF_R {
+        ANMF_R::new(((self.bits[1] >> 31) & 0x01) != 0)
+    }
+    pub fn to_data_length(&self) -> DataLength {
+        let dlc = self.dlc().bits();
+        let ff = self.fdf().frame_format();
+        let len = if ff == FrameFormat::Fdcan {
+            // See RM0433 Rev 7 Table 475. DLC coding
+            match dlc {
+                0..=8 => dlc,
+                9 => 12,
+                10 => 16,
+                11 => 20,
+                12 => 24,
+                13 => 32,
+                14 => 48,
+                15 => 64,
+                _ => panic!("DLC > 15"),
+            }
+        } else {
+            match dlc {
+                0..=8 => dlc,
+                9..=15 => 8,
+                _ => panic!("DLC > 15"),
+            }
+        };
+        DataLength::new(len, ff)
+    }
+    pub fn to_filter_match(&self) -> FilterFrameMatch {
+        if self.anmf().is_matching_frame() {
+            FilterFrameMatch::DidMatch(self.fidx().bits())
+        } else {
+            FilterFrameMatch::DidNotMatch
+        }
+    }
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/standard_filter.rs b/embassy-stm32/src/can/fd/message_ram/standard_filter.rs
new file mode 100644
index 000000000..3a3bbcf12
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/standard_filter.rs
@@ -0,0 +1,136 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+#![allow(non_camel_case_types)]
+#![allow(non_snake_case)]
+#![allow(unused)]
+
+use super::common::{ESFEC_R, ESFT_R};
+use super::enums::{FilterElementConfig, FilterType};
+use super::generic;
+
+#[doc = "Reader of register StandardFilter"]
+pub(crate) type R = generic::R<super::StandardFilterType, super::StandardFilter>;
+#[doc = "Writer for register StandardFilter"]
+pub(crate) type W = generic::W<super::StandardFilterType, super::StandardFilter>;
+#[doc = "Register StandardFilter `reset()`'s with value 0xC0000"]
+impl generic::ResetValue for super::StandardFilter {
+    type Type = super::StandardFilterType;
+    #[inline(always)]
+    fn reset_value() -> Self::Type {
+        // Sets filter element to Disabled
+        0xC000
+    }
+}
+
+#[doc = "Reader of field `SFID2`"]
+pub(crate) type SFID2_R = generic::R<u16, u16>;
+#[doc = "Write proxy for field `SFID2`"]
+pub(crate) struct SFID2_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> SFID2_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u16) -> &'a mut W {
+        self.w.bits = (self.w.bits & !(0x07ff)) | ((value as u32) & 0x07ff);
+        self.w
+    }
+}
+
+#[doc = "Reader of field `SFID1`"]
+pub(crate) type SFID1_R = generic::R<u16, u16>;
+#[doc = "Write proxy for field `SFID1`"]
+pub(crate) struct SFID1_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> SFID1_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u16) -> &'a mut W {
+        self.w.bits = (self.w.bits & !(0x07ff << 16)) | (((value as u32) & 0x07ff) << 16);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `SFEC`"]
+pub(crate) struct SFEC_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> SFEC_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u8) -> &'a mut W {
+        self.w.bits = (self.w.bits & !(0x07 << 27)) | (((value as u32) & 0x07) << 27);
+        self.w
+    }
+    #[doc = r"Sets the field according to FilterElementConfig"]
+    #[inline(always)]
+    pub fn set_filter_element_config(self, fec: FilterElementConfig) -> &'a mut W {
+        //SAFETY: FilterElementConfig only be valid options
+        unsafe { self.bits(fec as u8) }
+    }
+}
+
+#[doc = "Write proxy for field `SFT`"]
+pub(crate) struct SFT_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> SFT_W<'a> {
+    #[doc = r"Sets the field according the FilterType"]
+    #[inline(always)]
+    pub fn set_filter_type(self, filter: FilterType) -> &'a mut W {
+        //SAFETY: FilterType only be valid options
+        unsafe { self.bits(filter as u8) }
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u8) -> &'a mut W {
+        self.w.bits = (self.w.bits & !(0x03 << 30)) | (((value as u32) & 0x03) << 30);
+        self.w
+    }
+}
+
+impl R {
+    #[doc = "Bits 0:10 - SFID2"]
+    #[inline(always)]
+    pub fn sfid2(&self) -> SFID2_R {
+        SFID2_R::new((self.bits & 0x07ff) as u16)
+    }
+    #[doc = "Bits 16:26 - SFID1"]
+    #[inline(always)]
+    pub fn sfid1(&self) -> SFID1_R {
+        SFID1_R::new(((self.bits >> 16) & 0x07ff) as u16)
+    }
+    #[doc = "Bits 27:29 - SFEC"]
+    #[inline(always)]
+    pub fn sfec(&self) -> ESFEC_R {
+        ESFEC_R::new(((self.bits >> 27) & 0x07) as u8)
+    }
+    #[doc = "Bits 30:31 - SFT"]
+    #[inline(always)]
+    pub fn sft(&self) -> ESFT_R {
+        ESFT_R::new(((self.bits >> 30) & 0x03) as u8)
+    }
+}
+impl W {
+    #[doc = "Bits 0:10 - SFID2"]
+    #[inline(always)]
+    pub fn sfid2(&mut self) -> SFID2_W {
+        SFID2_W { w: self }
+    }
+    #[doc = "Bits 16:26 - SFID1"]
+    #[inline(always)]
+    pub fn sfid1(&mut self) -> SFID1_W {
+        SFID1_W { w: self }
+    }
+    #[doc = "Bits 27:29 - SFEC"]
+    #[inline(always)]
+    pub fn sfec(&mut self) -> SFEC_W {
+        SFEC_W { w: self }
+    }
+    #[doc = "Bits 30:31 - SFT"]
+    #[inline(always)]
+    pub fn sft(&mut self) -> SFT_W {
+        SFT_W { w: self }
+    }
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/txbuffer_element.rs b/embassy-stm32/src/can/fd/message_ram/txbuffer_element.rs
new file mode 100644
index 000000000..455406a1c
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/txbuffer_element.rs
@@ -0,0 +1,433 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+#![allow(non_camel_case_types)]
+#![allow(non_snake_case)]
+#![allow(unused)]
+
+use super::common::{BRS_R, DLC_R, ESI_R, FDF_R, ID_R, RTR_R, XTD_R};
+use super::enums::{
+    BitRateSwitching, DataLength, ErrorStateIndicator, Event, EventControl, FrameFormat, IdType,
+    RemoteTransmissionRequest,
+};
+use super::generic;
+
+#[doc = "Reader of register TxBufferElement"]
+pub(crate) type R = generic::R<super::TxBufferElementHeaderType, super::TxBufferElementHeader>;
+#[doc = "Writer for register TxBufferElement"]
+pub(crate) type W = generic::W<super::TxBufferElementHeaderType, super::TxBufferElementHeader>;
+impl generic::ResetValue for super::TxBufferElementHeader {
+    type Type = super::TxBufferElementHeaderType;
+
+    #[allow(dead_code)]
+    #[inline(always)]
+    fn reset_value() -> Self::Type {
+        [0; 2]
+    }
+}
+
+#[doc = "Write proxy for field `ESI`"]
+pub(crate) struct ESI_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> ESI_W<'a> {
+    #[doc = r"Writes `variant` to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_error_indicator(self, esi: ErrorStateIndicator) -> &'a mut W {
+        self.bit(esi as u8 != 0)
+    }
+
+    #[doc = r"Sets the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_bit(self) -> &'a mut W {
+        self.bit(true)
+    }
+    #[doc = r"Clears the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn clear_bit(self) -> &'a mut W {
+        self.bit(false)
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn bit(self, value: bool) -> &'a mut W {
+        self.w.bits[0] = (self.w.bits[0] & !(0x01 << 31)) | (((value as u32) & 0x01) << 31);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `XTD`"]
+pub(crate) struct XTD_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> XTD_W<'a> {
+    #[doc = r"Writes `variant` to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_id_type(self, idt: IdType) -> &'a mut W {
+        self.bit(idt as u8 != 0)
+    }
+
+    #[doc = r"Sets the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_bit(self) -> &'a mut W {
+        self.bit(true)
+    }
+    #[doc = r"Clears the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn clear_bit(self) -> &'a mut W {
+        self.bit(false)
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn bit(self, value: bool) -> &'a mut W {
+        self.w.bits[0] = (self.w.bits[0] & !(0x01 << 30)) | (((value as u32) & 0x01) << 30);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `RTR`"]
+pub(crate) struct RTR_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> RTR_W<'a> {
+    #[doc = r"Writes `variant` to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_rtr(self, rtr: RemoteTransmissionRequest) -> &'a mut W {
+        self.bit(rtr as u8 != 0)
+    }
+
+    #[doc = r"Sets the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_bit(self) -> &'a mut W {
+        self.bit(true)
+    }
+    #[doc = r"Clears the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn clear_bit(self) -> &'a mut W {
+        self.bit(false)
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn bit(self, value: bool) -> &'a mut W {
+        self.w.bits[0] = (self.w.bits[0] & !(0x01 << 29)) | (((value as u32) & 0x01) << 29);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `ID`"]
+pub(crate) struct ID_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> ID_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub unsafe fn bits(self, value: u32) -> &'a mut W {
+        self.w.bits[0] = (self.w.bits[0] & !(0x1FFFFFFF)) | ((value as u32) & 0x1FFFFFFF);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `DLC`"]
+pub(crate) struct DLC_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> DLC_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub unsafe fn bits(self, value: u8) -> &'a mut W {
+        self.w.bits[1] = (self.w.bits[1] & !(0x0F << 16)) | (((value as u32) & 0x0F) << 16);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `BRS`"]
+pub(crate) struct BRS_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> BRS_W<'a> {
+    #[doc = r"Writes `variant` to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_brs(self, brs: BitRateSwitching) -> &'a mut W {
+        self.bit(brs as u8 != 0)
+    }
+
+    #[doc = r"Sets the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_bit(self) -> &'a mut W {
+        self.bit(true)
+    }
+    #[doc = r"Clears the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn clear_bit(self) -> &'a mut W {
+        self.bit(false)
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn bit(self, value: bool) -> &'a mut W {
+        self.w.bits[1] = (self.w.bits[1] & !(0x01 << 20)) | (((value as u32) & 0x01) << 20);
+        self.w
+    }
+}
+
+#[doc = "Write proxy for field `FDF`"]
+pub(crate) struct FDF_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> FDF_W<'a> {
+    #[doc = r"Writes `variant` to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_format(self, fdf: FrameFormat) -> &'a mut W {
+        self.bit(fdf as u8 != 0)
+    }
+
+    #[doc = r"Sets the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_bit(self) -> &'a mut W {
+        self.bit(true)
+    }
+    #[doc = r"Clears the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn clear_bit(self) -> &'a mut W {
+        self.bit(false)
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn bit(self, value: bool) -> &'a mut W {
+        self.w.bits[1] = (self.w.bits[1] & !(0x01 << 21)) | (((value as u32) & 0x01) << 21);
+        self.w
+    }
+}
+
+#[doc = "Reader of field `EFC`"]
+pub(crate) type EFC_R = generic::R<bool, EventControl>;
+impl EFC_R {
+    pub fn to_event_control(&self) -> EventControl {
+        match self.bit() {
+            false => EventControl::DoNotStore,
+            true => EventControl::Store,
+        }
+    }
+}
+#[doc = "Write proxy for field `EFC`"]
+pub(crate) struct EFC_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> EFC_W<'a> {
+    #[doc = r"Writes `variant` to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_event_control(self, efc: EventControl) -> &'a mut W {
+        self.bit(match efc {
+            EventControl::DoNotStore => false,
+            EventControl::Store => true,
+        })
+    }
+
+    #[doc = r"Sets the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn set_bit(self) -> &'a mut W {
+        self.bit(true)
+    }
+    #[doc = r"Clears the field bit"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn clear_bit(self) -> &'a mut W {
+        self.bit(false)
+    }
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    #[allow(dead_code)]
+    pub fn bit(self, value: bool) -> &'a mut W {
+        self.w.bits[1] = (self.w.bits[1] & !(0x01 << 23)) | (((value as u32) & 0x01) << 23);
+        self.w
+    }
+}
+
+struct Marker(u8);
+impl From<Event> for Marker {
+    fn from(e: Event) -> Marker {
+        match e {
+            Event::NoEvent => Marker(0),
+            Event::Event(mm) => Marker(mm),
+        }
+    }
+}
+
+#[doc = "Reader of field `MM`"]
+pub(crate) type MM_R = generic::R<u8, u8>;
+#[doc = "Write proxy for field `MM`"]
+pub(crate) struct MM_W<'a> {
+    w: &'a mut W,
+}
+impl<'a> MM_W<'a> {
+    #[doc = r"Writes raw bits to the field"]
+    #[inline(always)]
+    pub unsafe fn bits(self, value: u8) -> &'a mut W {
+        self.w.bits[1] = (self.w.bits[1] & !(0x7F << 24)) | (((value as u32) & 0x7F) << 24);
+        self.w
+    }
+
+    fn set_message_marker(self, mm: Marker) -> &'a mut W {
+        unsafe { self.bits(mm.0) }
+    }
+}
+
+impl R {
+    #[doc = "Byte 0 - Bits 0:28 - ID"]
+    #[inline(always)]
+    pub fn id(&self) -> ID_R {
+        ID_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32)
+    }
+    #[doc = "Byte 0 - Bit 29 - RTR"]
+    #[inline(always)]
+    pub fn rtr(&self) -> RTR_R {
+        RTR_R::new(((self.bits[0] >> 29) & 0x01) != 0)
+    }
+    #[doc = "Byte 0 - Bit 30 - XTD"]
+    #[inline(always)]
+    pub fn xtd(&self) -> XTD_R {
+        XTD_R::new(((self.bits[0] >> 30) & 0x01) != 0)
+    }
+    #[doc = "Byte 0 - Bit 30 - ESI"]
+    #[inline(always)]
+    pub fn esi(&self) -> ESI_R {
+        ESI_R::new(((self.bits[0] >> 31) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 16:19 - DLC"]
+    #[inline(always)]
+    pub fn dlc(&self) -> DLC_R {
+        DLC_R::new(((self.bits[1] >> 16) & 0x0F) as u8)
+    }
+    #[doc = "Byte 1 - Bits 20 - BRS"]
+    #[inline(always)]
+    pub fn brs(&self) -> BRS_R {
+        BRS_R::new(((self.bits[1] >> 20) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 20 - FDF"]
+    #[inline(always)]
+    pub fn fdf(&self) -> FDF_R {
+        FDF_R::new(((self.bits[1] >> 21) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 23 - EFC"]
+    #[inline(always)]
+    pub fn efc(&self) -> EFC_R {
+        EFC_R::new(((self.bits[1] >> 23) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 24:31 - MM"]
+    #[inline(always)]
+    pub fn mm(&self) -> MM_R {
+        MM_R::new(((self.bits[1] >> 24) & 0xFF) as u8)
+    }
+    pub fn to_data_length(&self) -> DataLength {
+        let dlc = self.dlc().bits();
+        let ff = self.fdf().frame_format();
+        let len = if ff == FrameFormat::Fdcan {
+            // See RM0433 Rev 7 Table 475. DLC coding
+            match dlc {
+                0..=8 => dlc,
+                9 => 12,
+                10 => 16,
+                11 => 20,
+                12 => 24,
+                13 => 32,
+                14 => 48,
+                15 => 64,
+                _ => panic!("DLC > 15"),
+            }
+        } else {
+            match dlc {
+                0..=8 => dlc,
+                9..=15 => 8,
+                _ => panic!("DLC > 15"),
+            }
+        };
+        DataLength::new(len, ff)
+    }
+    pub fn to_event(&self) -> Event {
+        let mm = self.mm().bits();
+        let efc = self.efc().to_event_control();
+        match efc {
+            EventControl::DoNotStore => Event::NoEvent,
+            EventControl::Store => Event::Event(mm),
+        }
+    }
+}
+impl W {
+    #[doc = "Byte 0 - Bits 0:28 - ID"]
+    #[inline(always)]
+    pub fn id(&mut self) -> ID_W {
+        ID_W { w: self }
+    }
+    #[doc = "Byte 0 - Bit 29 - RTR"]
+    #[inline(always)]
+    pub fn rtr(&mut self) -> RTR_W {
+        RTR_W { w: self }
+    }
+    #[doc = "Byte 0 - Bit 30 - XTD"]
+    #[inline(always)]
+    pub fn xtd(&mut self) -> XTD_W {
+        XTD_W { w: self }
+    }
+    #[doc = "Byte 0 - Bit 31 - ESI"]
+    #[inline(always)]
+    pub fn esi(&mut self) -> ESI_W {
+        ESI_W { w: self }
+    }
+    #[doc = "Byte 1 - Bit 16:19 - DLC"]
+    #[inline(always)]
+    pub fn dlc(&mut self) -> DLC_W {
+        DLC_W { w: self }
+    }
+    #[doc = "Byte 1 - Bit 20 - BRS"]
+    #[inline(always)]
+    pub fn brs(&mut self) -> BRS_W {
+        BRS_W { w: self }
+    }
+    #[doc = "Byte 1 - Bit 21 - FDF"]
+    #[inline(always)]
+    pub fn fdf(&mut self) -> FDF_W {
+        FDF_W { w: self }
+    }
+    #[doc = "Byte 1 - Bit 23 - EFC"]
+    #[inline(always)]
+    pub fn efc(&mut self) -> EFC_W {
+        EFC_W { w: self }
+    }
+    #[doc = "Byte 1 - Bit 24:31 - MM"]
+    #[inline(always)]
+    pub fn mm(&mut self) -> MM_W {
+        MM_W { w: self }
+    }
+    #[doc = "Convenience function for setting the data length and frame format"]
+    #[inline(always)]
+    pub fn set_len(&mut self, dl: impl Into<DataLength>) -> &mut Self {
+        let dl: DataLength = dl.into();
+        self.fdf().set_format(dl.into());
+        unsafe { self.dlc().bits(dl.dlc()) }
+    }
+    pub fn set_event(&mut self, event: Event) -> &mut Self {
+        self.mm().set_message_marker(event.into());
+        self.efc().set_event_control(event.into())
+    }
+}
diff --git a/embassy-stm32/src/can/fd/message_ram/txevent_element.rs b/embassy-stm32/src/can/fd/message_ram/txevent_element.rs
new file mode 100644
index 000000000..817a4449f
--- /dev/null
+++ b/embassy-stm32/src/can/fd/message_ram/txevent_element.rs
@@ -0,0 +1,138 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+#![allow(non_camel_case_types)]
+#![allow(non_snake_case)]
+#![allow(unused)]
+
+use super::common::{BRS_R, DLC_R, ESI_R, RTR_R, XTD_R};
+use super::generic;
+
+#[doc = "Reader of register TxEventElement"]
+pub(crate) type R = generic::R<super::TxEventElementType, super::TxEventElement>;
+// #[doc = "Writer for register TxEventElement"]
+// pub(crate) type W = generic::W<super::TxEventElementType, super::TxEventElement>;
+#[doc = "Register TxEventElement `reset()`'s"]
+impl generic::ResetValue for super::TxEventElement {
+    type Type = super::TxEventElementType;
+    #[inline(always)]
+    fn reset_value() -> Self::Type {
+        [0, 0]
+    }
+}
+
+#[doc = "Reader of field `ID`"]
+pub(crate) type ID_R = generic::R<u32, u32>;
+
+#[doc = "Reader of field `TXTS`"]
+pub(crate) type TXTS_R = generic::R<u16, u16>;
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub(crate) enum DataLengthFormat {
+    StandardLength = 0,
+    FDCANLength = 1,
+}
+impl From<DataLengthFormat> for bool {
+    #[inline(always)]
+    fn from(dlf: DataLengthFormat) -> Self {
+        dlf as u8 != 0
+    }
+}
+
+#[doc = "Reader of field `EDL`"]
+pub(crate) type EDL_R = generic::R<bool, DataLengthFormat>;
+impl EDL_R {
+    pub fn data_length_format(&self) -> DataLengthFormat {
+        match self.bits() {
+            false => DataLengthFormat::StandardLength,
+            true => DataLengthFormat::FDCANLength,
+        }
+    }
+    pub fn is_standard_length(&self) -> bool {
+        *self == DataLengthFormat::StandardLength
+    }
+    pub fn is_fdcan_length(&self) -> bool {
+        *self == DataLengthFormat::FDCANLength
+    }
+}
+
+#[derive(Clone, Copy, Debug, PartialEq)]
+pub(crate) enum EventType {
+    //_Reserved = 0b00,
+    TxEvent = 0b01,
+    TxDespiteAbort = 0b10,
+    //_Reserved = 0b10,
+}
+
+#[doc = "Reader of field `EFC`"]
+pub(crate) type EFC_R = generic::R<u8, EventType>;
+impl EFC_R {
+    pub fn event_type(&self) -> EventType {
+        match self.bits() {
+            0b01 => EventType::TxEvent,
+            0b10 => EventType::TxDespiteAbort,
+            _ => unimplemented!(),
+        }
+    }
+    pub fn is_tx_event(&self) -> bool {
+        self.event_type() == EventType::TxEvent
+    }
+    pub fn is_despite_abort(&self) -> bool {
+        self.event_type() == EventType::TxDespiteAbort
+    }
+}
+
+#[doc = "Reader of field `MM`"]
+pub(crate) type MM_R = generic::R<u8, u8>;
+
+impl R {
+    #[doc = "Byte 0 - Bits 0:28 - ID"]
+    #[inline(always)]
+    pub fn id(&self) -> ID_R {
+        ID_R::new(((self.bits[0]) & 0x1FFFFFFF) as u32)
+    }
+    #[doc = "Byte 0 - Bit 29 - RTR"]
+    #[inline(always)]
+    pub fn rtr(&self) -> RTR_R {
+        RTR_R::new(((self.bits[0] >> 29) & 0x01) != 0)
+    }
+    #[doc = "Byte 0 - Bit 30 - XTD"]
+    #[inline(always)]
+    pub fn xtd(&self) -> XTD_R {
+        XTD_R::new(((self.bits[0] >> 30) & 0x01) != 0)
+    }
+    #[doc = "Byte 0 - Bit 30 - ESI"]
+    #[inline(always)]
+    pub fn esi(&self) -> ESI_R {
+        ESI_R::new(((self.bits[0] >> 31) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 0:15 - TXTS"]
+    #[inline(always)]
+    pub fn txts(&self) -> TXTS_R {
+        TXTS_R::new(((self.bits[1]) & 0xFFFF) as u16)
+    }
+    #[doc = "Byte 1 - Bits 16:19 - DLC"]
+    #[inline(always)]
+    pub fn dlc(&self) -> DLC_R {
+        DLC_R::new(((self.bits[1] >> 16) & 0x0F) as u8)
+    }
+    #[doc = "Byte 1 - Bits 20 - BRS"]
+    #[inline(always)]
+    pub fn brs(&self) -> BRS_R {
+        BRS_R::new(((self.bits[1] >> 20) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 21 - EDL"]
+    #[inline(always)]
+    pub fn edl(&self) -> EDL_R {
+        EDL_R::new(((self.bits[1] >> 21) & 0x01) != 0)
+    }
+    #[doc = "Byte 1 - Bits 22:23 - EFC"]
+    #[inline(always)]
+    pub fn efc(&self) -> EFC_R {
+        EFC_R::new(((self.bits[1] >> 22) & 0x03) as u8)
+    }
+    #[doc = "Byte 1 - Bits 24:31 - MM"]
+    #[inline(always)]
+    pub fn mm(&self) -> MM_R {
+        MM_R::new(((self.bits[1] >> 24) & 0xFF) as u8)
+    }
+}
diff --git a/embassy-stm32/src/can/fd/mod.rs b/embassy-stm32/src/can/fd/mod.rs
new file mode 100644
index 000000000..271ca0b3c
--- /dev/null
+++ b/embassy-stm32/src/can/fd/mod.rs
@@ -0,0 +1,6 @@
+//! Module containing that which is specific to fdcan hardware variant
+
+pub mod config;
+pub mod filter;
+pub(crate) mod message_ram;
+pub(crate) mod peripheral;
diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs
new file mode 100644
index 000000000..0771d6fbb
--- /dev/null
+++ b/embassy-stm32/src/can/fd/peripheral.rs
@@ -0,0 +1,828 @@
+// Note: This file is copied and modified from fdcan crate by Richard Meadows
+
+use core::convert::Infallible;
+use core::slice;
+
+use cfg_if::cfg_if;
+
+use crate::can::enums::*;
+use crate::can::fd::config::*;
+use crate::can::fd::message_ram::enums::*;
+use crate::can::fd::message_ram::{RegisterBlock, RxFifoElement, TxBufferElement};
+use crate::can::frame::*;
+
+/// Loopback Mode
+#[derive(Clone, Copy, Debug)]
+enum LoopbackMode {
+    None,
+    Internal,
+    External,
+}
+
+pub struct Registers {
+    pub regs: &'static crate::pac::can::Fdcan,
+    pub msgram: &'static crate::pac::fdcanram::Fdcanram,
+}
+
+impl Registers {
+    fn tx_buffer_element(&self, bufidx: usize) -> &mut TxBufferElement {
+        &mut self.msg_ram_mut().transmit.tbsa[bufidx]
+    }
+    pub fn msg_ram_mut(&self) -> &mut RegisterBlock {
+        let ptr = self.msgram.as_ptr() as *mut RegisterBlock;
+        unsafe { &mut (*ptr) }
+    }
+
+    fn rx_fifo_element(&self, fifonr: usize, bufnum: usize) -> &mut RxFifoElement {
+        &mut self.msg_ram_mut().receive[fifonr].fxsa[bufnum]
+    }
+
+    pub fn read_classic(&self, fifonr: usize) -> Option<(ClassicFrame, 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; 8] = [0; 8];
+        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 = 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))
+            }
+            None => None,
+        }
+    }
+
+    pub fn put_tx_frame(&self, bufidx: usize, header: &Header, buffer: &[u8]) {
+        // 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.tx_buffer_element(bufidx);
+
+        mailbox.reset();
+        put_tx_header(mailbox, header);
+        put_tx_data(mailbox, &buffer[..header.len() as usize]);
+
+        // Set <idx as Mailbox> as ready to transmit
+        self.regs.txbar().modify(|w| w.set_ar(bufidx, true));
+    }
+
+    fn reg_to_error(value: u8) -> Option<BusError> {
+        match value {
+            //0b000 => None,
+            0b001 => Some(BusError::Stuff),
+            0b010 => Some(BusError::Form),
+            0b011 => Some(BusError::Acknowledge),
+            0b100 => Some(BusError::BitRecessive),
+            0b101 => Some(BusError::BitDominant),
+            0b110 => Some(BusError::Crc),
+            //0b111 => Some(BusError::NoError),
+            _ => None,
+        }
+    }
+
+    pub fn curr_error(&self) -> Option<BusError> {
+        let err = { self.regs.psr().read() };
+        if err.bo() {
+            return Some(BusError::BusOff);
+        } else if err.ep() {
+            return Some(BusError::BusPassive);
+        } else if err.ew() {
+            return Some(BusError::BusWarning);
+        } else {
+            cfg_if! {
+                if #[cfg(stm32h7)] {
+                    let lec = err.lec();
+                } else {
+                    let lec = err.lec().to_bits();
+                }
+            }
+            if let Some(err) = Self::reg_to_error(lec) {
+                return Some(err);
+            }
+        }
+        None
+    }
+    /// Returns if the tx queue is able to accept new messages without having to cancel an existing one
+    #[inline]
+    pub fn tx_queue_is_full(&self) -> bool {
+        self.regs.txfqs().read().tfqf()
+    }
+
+    #[inline]
+    pub fn has_pending_frame(&self, idx: usize) -> bool {
+        self.regs.txbrp().read().trp(idx)
+    }
+
+    /// Returns `Ok` when the mailbox is free or if it contains pending frame with a
+    /// lower priority (higher ID) than the identifier `id`.
+    #[inline]
+    pub fn is_available(&self, bufidx: usize, id: &embedded_can::Id) -> bool {
+        if self.has_pending_frame(bufidx) {
+            let mailbox = self.tx_buffer_element(bufidx);
+
+            let header_reg = mailbox.header.read();
+            let old_id = make_id(header_reg.id().bits(), header_reg.xtd().bits());
+
+            *id > old_id
+        } else {
+            true
+        }
+    }
+
+    /// 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`.
+    #[inline]
+    pub fn abort(&self, bufidx: usize) -> bool {
+        let can = self.regs;
+
+        // Check if there is a request pending to abort
+        if self.has_pending_frame(bufidx) {
+            // Abort Request
+            can.txbcr().write(|w| w.set_cr(bufidx, true));
+
+            // Wait for the abort request to be finished.
+            loop {
+                if can.txbcf().read().cf(bufidx) {
+                    // Return false when a transmission has occured
+                    break can.txbto().read().to(bufidx) == false;
+                }
+            }
+        } else {
+            false
+        }
+    }
+
+    #[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,
+    {
+        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 > ClassicFrame::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 = 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())
+            } else {
+                Header::new(id, len, header_reg.rtr().bit())
+            };
+
+            Some(FdFrame::new(header, 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
+        }
+    }
+
+    /// 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> {
+        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_fd_mailbox(0))
+            } else if self.is_available(1, id) {
+                (1, self.abort_pending_fd_mailbox(1))
+            } else if self.is_available(2, id) {
+                (2, self.abort_pending_fd_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)
+    }
+
+    #[inline]
+    fn reset_msg_ram(&mut self) {
+        self.msg_ram_mut().reset();
+    }
+
+    #[inline]
+    fn enter_init_mode(&mut self) {
+        self.regs.cccr().modify(|w| w.set_init(true));
+        while false == self.regs.cccr().read().init() {}
+        self.regs.cccr().modify(|w| w.set_cce(true));
+    }
+
+    /// Enables or disables loopback mode: Internally connects the TX and RX
+    /// signals together.
+    #[inline]
+    fn set_loopback_mode(&mut self, mode: LoopbackMode) {
+        let (test, mon, lbck) = match mode {
+            LoopbackMode::None => (false, false, false),
+            LoopbackMode::Internal => (true, true, true),
+            LoopbackMode::External => (true, false, true),
+        };
+
+        self.set_test_mode(test);
+        self.set_bus_monitoring_mode(mon);
+
+        self.regs.test().modify(|w| w.set_lbck(lbck));
+    }
+
+    /// Enables or disables silent mode: Disconnects the TX signal from the pin.
+    #[inline]
+    fn set_bus_monitoring_mode(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_mon(enabled));
+    }
+
+    #[inline]
+    fn set_restricted_operations(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_asm(enabled));
+    }
+
+    #[inline]
+    fn set_normal_operations(&mut self, _enabled: bool) {
+        self.set_loopback_mode(LoopbackMode::None);
+    }
+
+    #[inline]
+    fn set_test_mode(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_test(enabled));
+    }
+
+    #[inline]
+    fn set_power_down_mode(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_csr(enabled));
+        while self.regs.cccr().read().csa() != enabled {}
+    }
+
+    /// Moves out of PoweredDownMode and into ConfigMode
+    #[inline]
+    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
+        assert!(
+            self.regs.crel().read().rel() == 3,
+            "Expected FDCAN core major release 3"
+        );
+        assert!(
+            self.regs.endn().read().etv() == 0x87654321_u32,
+            "Error reading endianness test value from FDCAN core"
+        );
+
+        // Framework specific settings are set here
+
+        // set TxBuffer to Queue Mode
+        self.regs.txbc().write(|w| w.set_tfqm(true));
+
+        // set standard filters list size to 28
+        // set extended filters list size to 8
+        // REQUIRED: we use the memory map as if these settings are set
+        // instead of re-calculating them.
+        #[cfg(not(stm32h7))]
+        {
+            self.regs.rxgfc().modify(|w| {
+                w.set_lss(crate::can::fd::message_ram::STANDARD_FILTER_MAX);
+                w.set_lse(crate::can::fd::message_ram::EXTENDED_FILTER_MAX);
+            });
+        }
+        #[cfg(stm32h7)]
+        {
+            self.regs
+                .sidfc()
+                .modify(|w| w.set_lss(crate::can::fd::message_ram::STANDARD_FILTER_MAX));
+            self.regs
+                .xidfc()
+                .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());
+
+        //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_data_bit_timing(config.dbtr);
+        self.set_nominal_bit_timing(config.nbtr);
+        self.set_automatic_retransmit(config.automatic_retransmit);
+        self.set_transmit_pause(config.transmit_pause);
+        self.set_frame_transmit(config.frame_transmit);
+        //self.set_interrupt_line_config(config.interrupt_line_config);
+        self.set_non_iso_mode(config.non_iso_mode);
+        self.set_edge_filtering(config.edge_filtering);
+        self.set_protocol_exception_handling(config.protocol_exception_handling);
+        self.set_global_filter(config.global_filter);
+    }
+
+    #[inline]
+    fn leave_init_mode(&mut self, config: FdCanConfig) {
+        self.apply_config(config);
+
+        self.regs.cccr().modify(|w| w.set_cce(false));
+        self.regs.cccr().modify(|w| w.set_init(false));
+        while self.regs.cccr().read().init() == true {}
+    }
+
+    /// Moves out of ConfigMode and into specified mode
+    #[inline]
+    pub fn into_mode(mut self, config: FdCanConfig, mode: crate::can::_version::FdcanOperatingMode) {
+        match mode {
+            crate::can::FdcanOperatingMode::InternalLoopbackMode => self.set_loopback_mode(LoopbackMode::Internal),
+            crate::can::FdcanOperatingMode::ExternalLoopbackMode => self.set_loopback_mode(LoopbackMode::External),
+            crate::can::FdcanOperatingMode::NormalOperationMode => self.set_normal_operations(true),
+            crate::can::FdcanOperatingMode::RestrictedOperationMode => self.set_restricted_operations(true),
+            crate::can::FdcanOperatingMode::BusMonitoringMode => self.set_bus_monitoring_mode(true),
+        }
+        self.leave_init_mode(config);
+    }
+
+    /// Moves out of ConfigMode and into InternalLoopbackMode
+    #[inline]
+    pub fn into_internal_loopback(mut self, config: FdCanConfig) {
+        self.set_loopback_mode(LoopbackMode::Internal);
+        self.leave_init_mode(config);
+    }
+
+    /// Moves out of ConfigMode and into ExternalLoopbackMode
+    #[inline]
+    pub fn into_external_loopback(mut self, config: FdCanConfig) {
+        self.set_loopback_mode(LoopbackMode::External);
+        self.leave_init_mode(config);
+    }
+
+    /// Moves out of ConfigMode and into RestrictedOperationMode
+    #[inline]
+    pub fn into_restricted(mut self, config: FdCanConfig) {
+        self.set_restricted_operations(true);
+        self.leave_init_mode(config);
+    }
+
+    /// Moves out of ConfigMode and into NormalOperationMode
+    #[inline]
+    pub fn into_normal(mut self, config: FdCanConfig) {
+        self.set_normal_operations(true);
+        self.leave_init_mode(config);
+    }
+
+    /// Moves out of ConfigMode and into BusMonitoringMode
+    #[inline]
+    pub fn into_bus_monitoring(mut self, config: FdCanConfig) {
+        self.set_bus_monitoring_mode(true);
+        self.leave_init_mode(config);
+    }
+
+    /// Moves out of ConfigMode and into Testmode
+    #[inline]
+    pub fn into_test_mode(mut self, config: FdCanConfig) {
+        self.set_test_mode(true);
+        self.leave_init_mode(config);
+    }
+
+    /// Moves out of ConfigMode and into PoweredDownmode
+    #[inline]
+    pub fn into_powered_down(mut self, config: FdCanConfig) {
+        self.set_power_down_mode(true);
+        self.leave_init_mode(config);
+    }
+
+    /// Configures the bit timings.
+    ///
+    /// You can use <http://www.bittiming.can-wiki.info/> to calculate the `btr` parameter. Enter
+    /// parameters as follows:
+    ///
+    /// - *Clock Rate*: The input clock speed to the CAN peripheral (*not* the CPU clock speed).
+    ///   This is the clock rate of the peripheral bus the CAN peripheral is attached to (eg. APB1).
+    /// - *Sample Point*: Should normally be left at the default value of 87.5%.
+    /// - *SJW*: Should normally be left at the default value of 1.
+    ///
+    /// Then copy the `CAN_BUS_TIME` register value from the table and pass it as the `btr`
+    /// parameter to this method.
+    #[inline]
+    pub fn set_nominal_bit_timing(&mut self, btr: NominalBitTiming) {
+        //self.control.config.nbtr = btr;
+
+        self.regs.nbtp().write(|w| {
+            w.set_nbrp(btr.nbrp() - 1);
+            w.set_ntseg1(btr.ntseg1() - 1);
+            w.set_ntseg2(btr.ntseg2() - 1);
+            w.set_nsjw(btr.nsjw() - 1);
+        });
+    }
+
+    /// Configures the data bit timings for the FdCan Variable Bitrates.
+    /// This is not used when frame_transmit is set to anything other than AllowFdCanAndBRS.
+    #[inline]
+    pub fn set_data_bit_timing(&mut self, btr: DataBitTiming) {
+        //self.control.config.dbtr = btr;
+
+        self.regs.dbtp().write(|w| {
+            w.set_dbrp(btr.dbrp() - 1);
+            w.set_dtseg1(btr.dtseg1() - 1);
+            w.set_dtseg2(btr.dtseg2() - 1);
+            w.set_dsjw(btr.dsjw() - 1);
+        });
+    }
+
+    /// Enables or disables automatic retransmission of messages
+    ///
+    /// If this is enabled, the CAN peripheral will automatically try to retransmit each frame
+    /// util it can be sent. Otherwise, it will try only once to send each frame.
+    ///
+    /// Automatic retransmission is enabled by default.
+    #[inline]
+    pub fn set_automatic_retransmit(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_dar(!enabled));
+        //self.control.config.automatic_retransmit = enabled;
+    }
+
+    /// Configures the transmit pause feature. See
+    /// [`FdCanConfig::set_transmit_pause`]
+    #[inline]
+    pub fn set_transmit_pause(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_txp(!enabled));
+        //self.control.config.transmit_pause = enabled;
+    }
+
+    /// Configures non-iso mode. See [`FdCanConfig::set_non_iso_mode`]
+    #[inline]
+    pub fn set_non_iso_mode(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_niso(enabled));
+        //self.control.config.non_iso_mode = enabled;
+    }
+
+    /// Configures edge filtering. See [`FdCanConfig::set_edge_filtering`]
+    #[inline]
+    pub fn set_edge_filtering(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_efbi(enabled));
+        //self.control.config.edge_filtering = enabled;
+    }
+
+    /// Configures frame transmission mode. See
+    /// [`FdCanConfig::set_frame_transmit`]
+    #[inline]
+    pub fn set_frame_transmit(&mut self, fts: FrameTransmissionConfig) {
+        let (fdoe, brse) = match fts {
+            FrameTransmissionConfig::ClassicCanOnly => (false, false),
+            FrameTransmissionConfig::AllowFdCan => (true, false),
+            FrameTransmissionConfig::AllowFdCanAndBRS => (true, true),
+        };
+
+        self.regs.cccr().modify(|w| {
+            w.set_fdoe(fdoe);
+            #[cfg(stm32h7)]
+            w.set_bse(brse);
+            #[cfg(not(stm32h7))]
+            w.set_brse(brse);
+        });
+
+        //self.control.config.frame_transmit = fts;
+    }
+
+    /// Sets the protocol exception handling on/off
+    #[inline]
+    pub fn set_protocol_exception_handling(&mut self, enabled: bool) {
+        self.regs.cccr().modify(|w| w.set_pxhd(!enabled));
+
+        //self.control.config.protocol_exception_handling = enabled;
+    }
+
+    /// Configures and resets the timestamp counter
+    #[inline]
+    pub fn set_timestamp_counter_source(&mut self, select: TimestampSource) {
+        #[cfg(stm32h7)]
+        let (tcp, tss) = match select {
+            TimestampSource::None => (0, 0),
+            TimestampSource::Prescaler(p) => (p as u8, 1),
+            TimestampSource::FromTIM3 => (0, 2),
+        };
+
+        #[cfg(not(stm32h7))]
+        let (tcp, tss) = match select {
+            TimestampSource::None => (0, stm32_metapac::can::vals::Tss::ZERO),
+            TimestampSource::Prescaler(p) => (p as u8, stm32_metapac::can::vals::Tss::INCREMENT),
+            TimestampSource::FromTIM3 => (0, stm32_metapac::can::vals::Tss::EXTERNAL),
+        };
+
+        self.regs.tscc().write(|w| {
+            w.set_tcp(tcp);
+            w.set_tss(tss);
+        });
+
+        //self.control.config.timestamp_source = select;
+    }
+
+    #[cfg(not(stm32h7))]
+    /// Configures the global filter settings
+    #[inline]
+    pub fn set_global_filter(&mut self, filter: GlobalFilter) {
+        let anfs = match filter.handle_standard_frames {
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => stm32_metapac::can::vals::Anfs::ACCEPT_FIFO_0,
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => stm32_metapac::can::vals::Anfs::ACCEPT_FIFO_1,
+            crate::can::fd::config::NonMatchingFilter::Reject => stm32_metapac::can::vals::Anfs::REJECT,
+        };
+        let anfe = match filter.handle_extended_frames {
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => stm32_metapac::can::vals::Anfe::ACCEPT_FIFO_0,
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => stm32_metapac::can::vals::Anfe::ACCEPT_FIFO_1,
+            crate::can::fd::config::NonMatchingFilter::Reject => stm32_metapac::can::vals::Anfe::REJECT,
+        };
+
+        self.regs.rxgfc().modify(|w| {
+            w.set_anfs(anfs);
+            w.set_anfe(anfe);
+            w.set_rrfs(filter.reject_remote_standard_frames);
+            w.set_rrfe(filter.reject_remote_extended_frames);
+        });
+    }
+
+    #[cfg(stm32h7)]
+    /// Configures the global filter settings
+    #[inline]
+    pub fn set_global_filter(&mut self, filter: GlobalFilter) {
+        let anfs = match filter.handle_standard_frames {
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => 0,
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => 1,
+            crate::can::fd::config::NonMatchingFilter::Reject => 2,
+        };
+
+        let anfe = match filter.handle_extended_frames {
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo0 => 0,
+            crate::can::fd::config::NonMatchingFilter::IntoRxFifo1 => 1,
+            crate::can::fd::config::NonMatchingFilter::Reject => 2,
+        };
+
+        self.regs.gfc().modify(|w| {
+            w.set_anfs(anfs);
+            w.set_anfe(anfe);
+            w.set_rrfs(filter.reject_remote_standard_frames);
+            w.set_rrfe(filter.reject_remote_extended_frames);
+        });
+    }
+}
+
+fn make_id(id: u32, extended: bool) -> embedded_can::Id {
+    if extended {
+        embedded_can::Id::from(unsafe { embedded_can::ExtendedId::new_unchecked(id & 0x1FFFFFFF) })
+    } else {
+        embedded_can::Id::from(unsafe { embedded_can::StandardId::new_unchecked((id & 0x000007FF) as u16) })
+    }
+}
+
+fn put_tx_header(mailbox: &mut TxBufferElement, header: &Header) {
+    let (id, id_type) = match header.id() {
+        embedded_can::Id::Standard(id) => (id.as_raw() as u32, IdType::StandardId),
+        embedded_can::Id::Extended(id) => (id.as_raw() as u32, IdType::ExtendedId),
+    };
+
+    // Use FDCAN only for DLC > 8. FDCAN users can revise this if required.
+    let frame_format = if header.len() > 8 || header.fdcan() {
+        FrameFormat::Fdcan
+    } else {
+        FrameFormat::Classic
+    };
+    let brs = header.len() > 8 || header.bit_rate_switching();
+
+    mailbox.header.write(|w| {
+        unsafe { w.id().bits(id) }
+            .rtr()
+            .bit(header.len() == 0 && header.rtr())
+            .xtd()
+            .set_id_type(id_type)
+            .set_len(DataLength::new(header.len(), frame_format))
+            .set_event(Event::NoEvent)
+            .fdf()
+            .set_format(frame_format)
+            .brs()
+            .bit(brs)
+        //esi.set_error_indicator(//TODO//)
+    });
+}
+
+fn put_tx_data(mailbox: &mut TxBufferElement, buffer: &[u8]) {
+    let mut lbuffer = [0_u32; 16];
+    let len = buffer.len();
+    let data = unsafe { slice::from_raw_parts_mut(lbuffer.as_mut_ptr() as *mut u8, len) };
+    data[..len].copy_from_slice(&buffer[..len]);
+    let data_len = ((len) + 3) / 4;
+    for (register, byte) in mailbox.data.iter_mut().zip(lbuffer[..data_len].iter()) {
+        unsafe { register.write(*byte) };
+    }
+}
+
+fn data_from_fifo(buffer: &mut [u8], mailbox: &RxFifoElement, len: usize) {
+    for (i, register) in mailbox.data.iter().enumerate() {
+        let register_value = register.read();
+        let register_bytes = unsafe { slice::from_raw_parts(&register_value as *const u32 as *const u8, 4) };
+        let num_bytes = (len) - i * 4;
+        if num_bytes <= 4 {
+            buffer[i * 4..i * 4 + num_bytes].copy_from_slice(&register_bytes[..num_bytes]);
+            break;
+        }
+        buffer[i * 4..(i + 1) * 4].copy_from_slice(register_bytes);
+    }
+}
+
+fn data_from_tx_buffer(buffer: &mut [u8], mailbox: &TxBufferElement, len: usize) {
+    for (i, register) in mailbox.data.iter().enumerate() {
+        let register_value = register.read();
+        let register_bytes = unsafe { slice::from_raw_parts(&register_value as *const u32 as *const u8, 4) };
+        let num_bytes = (len) - i * 4;
+        if num_bytes <= 4 {
+            buffer[i * 4..i * 4 + num_bytes].copy_from_slice(&register_bytes[..num_bytes]);
+            break;
+        }
+        buffer[i * 4..(i + 1) * 4].copy_from_slice(register_bytes);
+    }
+}
+
+impl From<&RxFifoElement> for ClassicFrame {
+    fn from(mailbox: &RxFifoElement) -> Self {
+        let header_reg = mailbox.header.read();
+
+        let id = make_id(header_reg.id().bits(), header_reg.xtd().bits());
+        let dlc = header_reg.to_data_length().len();
+        let len = dlc as usize;
+
+        let mut buffer: [u8; 64] = [0; 64];
+        data_from_fifo(&mut buffer, mailbox, len);
+        let data = ClassicData::new(&buffer[0..len]);
+        let header = Header::new(id, dlc, header_reg.rtr().bits());
+        ClassicFrame::new(header, data.unwrap())
+    }
+}
+
+fn extract_frame(mailbox: &RxFifoElement, buffer: &mut [u8]) -> Option<(Header, u16)> {
+    let header_reg = mailbox.header.read();
+
+    let id = make_id(header_reg.id().bits(), header_reg.xtd().bits());
+    let dlc = header_reg.to_data_length().len();
+    let len = dlc as usize;
+    let timestamp = header_reg.txts().bits;
+    if len > buffer.len() {
+        return None;
+    }
+    data_from_fifo(buffer, mailbox, len);
+    let header = if header_reg.fdf().bits {
+        Header::new_fd(id, dlc, header_reg.rtr().bits(), header_reg.brs().bits())
+    } else {
+        Header::new(id, dlc, header_reg.rtr().bits())
+    };
+    Some((header, timestamp))
+}