25 files changed, 4781 insertions, 807 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))
+}
diff --git a/embassy-stm32/src/can/fdcan.rs b/embassy-stm32/src/can/fdcan.rs
index faf4af73f..f1f6f935e 100644
--- a/embassy-stm32/src/can/fdcan.rs
+++ b/embassy-stm32/src/can/fdcan.rs
@@ -1,16 +1,16 @@
+#[allow(unused_variables)]
 use core::future::poll_fn;
 use core::marker::PhantomData;
-use core::ops::{Deref, DerefMut};
 use core::task::Poll;
 
-use cfg_if::cfg_if;
+pub mod fd;
 use embassy_hal_internal::{into_ref, PeripheralRef};
-pub use fdcan::frame::{FrameFormat, RxFrameInfo, TxFrameHeader};
-pub use fdcan::id::{ExtendedId, Id, StandardId};
-use fdcan::message_ram::RegisterBlock;
-use fdcan::{self, LastErrorCode};
-pub use fdcan::{config, filter};
+use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex;
+use embassy_sync::channel::Channel;
+use fd::config::*;
+use fd::filter::*;
 
+use crate::can::fd::peripheral::Registers;
 use crate::gpio::sealed::AFType;
 use crate::interrupt::typelevel::Interrupt;
 use crate::rcc::RccPeripheral;
@@ -20,127 +20,14 @@ pub mod enums;
 use enums::*;
 pub mod util;
 
-/// CAN Frame returned by read
-pub struct RxFrame {
-    /// CAN Header info: frame ID, data length and other meta
-    pub header: RxFrameInfo,
-    /// CAN(0-8 bytes) or FDCAN(0-64 bytes) Frame data
-    pub data: Data,
-    /// Reception time.
-    #[cfg(feature = "time")]
-    pub timestamp: embassy_time::Instant,
-}
-
-/// CAN frame used for write
-pub struct TxFrame {
-    /// CAN Header info: frame ID, data length and other meta
-    pub header: TxFrameHeader,
-    /// CAN(0-8 bytes) or FDCAN(0-64 bytes) Frame data
-    pub data: Data,
-}
-
-impl TxFrame {
-    /// Create new TX frame from header and data
-    pub fn new(header: TxFrameHeader, data: &[u8]) -> Option<Self> {
-        if data.len() < header.len as usize {
-            return None;
-        }
-
-        let Some(data) = Data::new(data) else { return None };
-
-        Some(TxFrame { header, data })
-    }
-
-    fn from_preserved(header: TxFrameHeader, data32: &[u32]) -> Option<Self> {
-        let mut data = [0u8; 64];
-
-        for i in 0..data32.len() {
-            data[4 * i..][..4].copy_from_slice(&data32[i].to_le_bytes());
-        }
-
-        let Some(data) = Data::new(&data) else { return None };
-
-        Some(TxFrame { header, data })
-    }
-
-    /// Access frame data. Slice length will match header.
-    pub fn data(&self) -> &[u8] {
-        &self.data.bytes[..(self.header.len as usize)]
-    }
-}
-
-impl RxFrame {
-    pub(crate) fn new(
-        header: RxFrameInfo,
-        data: &[u8],
-        #[cfg(feature = "time")] timestamp: embassy_time::Instant,
-    ) -> Self {
-        let data = Data::new(&data).unwrap_or_else(|| Data::empty());
+pub mod frame;
+use frame::*;
 
-        RxFrame {
-            header,
-            data,
-            #[cfg(feature = "time")]
-            timestamp,
-        }
-    }
-
-    /// Access frame data. Slice length will match header.
-    pub fn data(&self) -> &[u8] {
-        &self.data.bytes[..(self.header.len as usize)]
-    }
-}
-
-/// Payload of a (FD)CAN data frame.
-///
-/// Contains 0 to 64 Bytes of data.
-#[derive(Debug, Copy, Clone)]
-pub struct Data {
-    pub(crate) bytes: [u8; 64],
-}
-
-impl Data {
-    /// Creates a data payload from a raw byte slice.
-    ///
-    /// Returns `None` if `data` is more than 64 bytes (which is the maximum) or
-    /// cannot be represented with an FDCAN DLC.
-    pub fn new(data: &[u8]) -> Option<Self> {
-        if !Data::is_valid_len(data.len()) {
-            return None;
-        }
-
-        let mut bytes = [0; 64];
-        bytes[..data.len()].copy_from_slice(data);
-
-        Some(Self { bytes })
-    }
-
-    /// Raw read access to data.
-    pub fn raw(&self) -> &[u8] {
-        &self.bytes
-    }
+#[cfg(feature = "time")]
+type Timestamp = embassy_time::Instant;
 
-    /// Checks if the length can be encoded in FDCAN DLC field.
-    pub const fn is_valid_len(len: usize) -> bool {
-        match len {
-            0..=8 => true,
-            12 => true,
-            16 => true,
-            20 => true,
-            24 => true,
-            32 => true,
-            48 => true,
-            64 => true,
-            _ => false,
-        }
-    }
-
-    /// Creates an empty data payload containing 0 bytes.
-    #[inline]
-    pub const fn empty() -> Self {
-        Self { bytes: [0; 64] }
-    }
-}
+#[cfg(not(feature = "time"))]
+type Timestamp = u16;
 
 /// Interrupt handler channel 0.
 pub struct IT0InterruptHandler<T: Instance> {
@@ -154,14 +41,37 @@ impl<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> for IT0Interrup
 
         let ir = regs.ir().read();
 
-        if ir.tc() {
-            regs.ir().write(|w| w.set_tc(true));
-            T::state().tx_waker.wake();
-        }
+        {
+            if ir.tc() {
+                regs.ir().write(|w| w.set_tc(true));
+            }
+            if ir.tefn() {
+                regs.ir().write(|w| w.set_tefn(true));
+            }
 
-        if ir.tefn() {
-            regs.ir().write(|w| w.set_tefn(true));
-            T::state().tx_waker.wake();
+            match &T::state().tx_mode {
+                sealed::TxMode::NonBuffered(waker) => waker.wake(),
+                sealed::TxMode::ClassicBuffered(buf) => {
+                    if !T::registers().tx_queue_is_full() {
+                        match buf.tx_receiver.try_receive() {
+                            Ok(frame) => {
+                                _ = T::registers().write_classic(&frame);
+                            }
+                            Err(_) => {}
+                        }
+                    }
+                }
+                sealed::TxMode::FdBuffered(buf) => {
+                    if !T::registers().tx_queue_is_full() {
+                        match buf.tx_receiver.try_receive() {
+                            Ok(frame) => {
+                                _ = T::registers().write_fd(&frame);
+                            }
+                            Err(_) => {}
+                        }
+                    }
+                }
+            }
         }
 
         if ir.ped() || ir.pea() {
@@ -172,13 +82,11 @@ impl<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> for IT0Interrup
         }
 
         if ir.rfn(0) {
-            regs.ir().write(|w| w.set_rfn(0, true));
-            T::state().rx_waker.wake();
+            T::state().rx_mode.on_interrupt::<T>(0);
         }
 
         if ir.rfn(1) {
-            regs.ir().write(|w| w.set_rfn(1, true));
-            T::state().rx_waker.wake();
+            T::state().rx_mode.on_interrupt::<T>(1);
         }
     }
 }
@@ -192,44 +100,56 @@ impl<T: Instance> interrupt::typelevel::Handler<T::IT1Interrupt> for IT1Interrup
     unsafe fn on_interrupt() {}
 }
 
-impl BusError {
-    fn try_from(lec: LastErrorCode) -> Option<BusError> {
-        match lec {
-            LastErrorCode::AckError => Some(BusError::Acknowledge),
-            // `0` data bit encodes a dominant state. `1` data bit is recessive.
-            // Bit0Error: During transmit, the node wanted to send a 0 but monitored a 1
-            LastErrorCode::Bit0Error => Some(BusError::BitRecessive),
-            LastErrorCode::Bit1Error => Some(BusError::BitDominant),
-            LastErrorCode::CRCError => Some(BusError::Crc),
-            LastErrorCode::FormError => Some(BusError::Form),
-            LastErrorCode::StuffError => Some(BusError::Stuff),
-            _ => None,
-        }
-    }
+#[derive(Debug, Copy, Clone, Eq, PartialEq)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+/// Different operating modes
+pub enum FdcanOperatingMode {
+    //PoweredDownMode,
+    //ConfigMode,
+    /// This mode can be used for a “Hot Selftest”, meaning the FDCAN can be tested without
+    /// affecting a running CAN system connected to the FDCAN_TX and FDCAN_RX pins. In this
+    /// mode, FDCAN_RX pin is disconnected from the FDCAN and FDCAN_TX pin is held
+    /// recessive.
+    InternalLoopbackMode,
+    /// This mode is provided for hardware self-test. To be independent from external stimulation,
+    /// the FDCAN ignores acknowledge errors (recessive bit sampled in the acknowledge slot of a
+    /// data / remote frame) in Loop Back mode. In this mode the FDCAN performs an internal
+    /// feedback from its transmit output to its receive input. The actual value of the FDCAN_RX
+    /// input pin is disregarded by the FDCAN. The transmitted messages can be monitored at the
+    /// FDCAN_TX transmit pin.
+    ExternalLoopbackMode,
+    /// The normal use of the Fdcan instance after configurations
+    NormalOperationMode,
+    /// In Restricted operation mode the node is able to receive data and remote frames and to give
+    /// acknowledge to valid frames, but it does not send data frames, remote frames, active error
+    /// frames, or overload frames. In case of an error condition or overload condition, it does not
+    /// send dominant bits, instead it waits for the occurrence of bus idle condition to resynchronize
+    /// itself to the CAN communication. The error counters for transmit and receive are frozen while
+    /// error logging (can_errors) is active. TODO: automatically enter in this mode?
+    RestrictedOperationMode,
+    ///  In Bus monitoring mode (for more details refer to ISO11898-1, 10.12 Bus monitoring),
+    /// the FDCAN is able to receive valid data frames and valid remote frames, but cannot start a
+    /// transmission. In this mode, it sends only recessive bits on the CAN bus. If the FDCAN is
+    /// required to send a dominant bit (ACK bit, overload flag, active error flag), the bit is
+    /// rerouted internally so that the FDCAN can monitor it, even if the CAN bus remains in recessive
+    /// state. In Bus monitoring mode the TXBRP register is held in reset state. The Bus monitoring
+    /// mode can be used to analyze the traffic on a CAN bus without affecting it by the transmission
+    /// of dominant bits.
+    BusMonitoringMode,
+    //TestMode,
 }
 
-/// Operating modes trait
-pub trait FdcanOperatingMode {}
-impl FdcanOperatingMode for fdcan::PoweredDownMode {}
-impl FdcanOperatingMode for fdcan::ConfigMode {}
-impl FdcanOperatingMode for fdcan::InternalLoopbackMode {}
-impl FdcanOperatingMode for fdcan::ExternalLoopbackMode {}
-impl FdcanOperatingMode for fdcan::NormalOperationMode {}
-impl FdcanOperatingMode for fdcan::RestrictedOperationMode {}
-impl FdcanOperatingMode for fdcan::BusMonitoringMode {}
-impl FdcanOperatingMode for fdcan::TestMode {}
-
 /// FDCAN Instance
-pub struct Fdcan<'d, T: Instance, M: FdcanOperatingMode> {
+pub struct FdcanConfigurator<'d, T: Instance> {
+    config: crate::can::fd::config::FdCanConfig,
     /// Reference to internals.
-    pub can: fdcan::FdCan<FdcanInstance<'d, T>, M>,
-    ns_per_timer_tick: u64, // For FDCAN internal timer
+    instance: FdcanInstance<'d, T>,
 }
 
-fn calc_ns_per_timer_tick<T: Instance>(mode: config::FrameTransmissionConfig) -> u64 {
+fn calc_ns_per_timer_tick<T: Instance>(mode: crate::can::fd::config::FrameTransmissionConfig) -> u64 {
     match mode {
         // Use timestamp from Rx FIFO to adjust timestamp reported to user
-        config::FrameTransmissionConfig::ClassicCanOnly => {
+        crate::can::fd::config::FrameTransmissionConfig::ClassicCanOnly => {
             let freq = T::frequency();
             let prescale: u64 =
                 ({ T::regs().nbtp().read().nbrp() } + 1) as u64 * ({ T::regs().tscc().read().tcp() } + 1) as u64;
@@ -241,42 +161,7 @@ fn calc_ns_per_timer_tick<T: Instance>(mode: config::FrameTransmissionConfig) ->
     }
 }
 
-#[cfg(feature = "time")]
-fn calc_timestamp<T: Instance>(ns_per_timer_tick: u64, ts_val: u16) -> embassy_time::Instant {
-    let now_embassy = embassy_time::Instant::now();
-    if ns_per_timer_tick == 0 {
-        return now_embassy;
-    }
-    let now_can = { T::regs().tscv().read().tsc() };
-    let delta = now_can.overflowing_sub(ts_val).0 as u64;
-    let ns = ns_per_timer_tick * delta as u64;
-    now_embassy - embassy_time::Duration::from_nanos(ns)
-}
-
-fn curr_error<T: Instance>() -> Option<BusError> {
-    let err = { T::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 Ok(err) = LastErrorCode::try_from(lec) {
-            return BusError::try_from(err);
-        }
-    }
-    None
-}
-
-impl<'d, T: Instance> Fdcan<'d, T, fdcan::ConfigMode> {
+impl<'d, T: Instance> FdcanConfigurator<'d, T> {
     /// Creates a new Fdcan instance, keeping the peripheral in sleep mode.
     /// You must call [Fdcan::enable_non_blocking] to use the peripheral.
     pub fn new(
@@ -286,7 +171,7 @@ impl<'d, T: Instance> Fdcan<'d, T, fdcan::ConfigMode> {
         _irqs: impl interrupt::typelevel::Binding<T::IT0Interrupt, IT0InterruptHandler<T>>
             + interrupt::typelevel::Binding<T::IT1Interrupt, IT1InterruptHandler<T>>
             + 'd,
-    ) -> Fdcan<'d, T, fdcan::ConfigMode> {
+    ) -> FdcanConfigurator<'d, T> {
         into_ref!(peri, rx, tx);
 
         rx.set_as_af(rx.af_num(), AFType::Input);
@@ -294,11 +179,12 @@ impl<'d, T: Instance> Fdcan<'d, T, fdcan::ConfigMode> {
 
         T::enable_and_reset();
 
+        let mut config = crate::can::fd::config::FdCanConfig::default();
+        T::registers().into_config_mode(config);
+
         rx.set_as_af(rx.af_num(), AFType::Input);
         tx.set_as_af(tx.af_num(), AFType::OutputPushPull);
 
-        let mut can = fdcan::FdCan::new(FdcanInstance(peri)).into_config_mode();
-
         T::configure_msg_ram();
         unsafe {
             // Enable timestamping
@@ -308,6 +194,7 @@ impl<'d, T: Instance> Fdcan<'d, T, fdcan::ConfigMode> {
                 .write(|w| w.set_tss(stm32_metapac::can::vals::Tss::INCREMENT));
             #[cfg(stm32h7)]
             T::regs().tscc().write(|w| w.set_tss(0x01));
+            config.timestamp_source = TimestampSource::Prescaler(TimestampPrescaler::_1);
 
             T::IT0Interrupt::unpend(); // Not unsafe
             T::IT0Interrupt::enable();
@@ -320,94 +207,134 @@ impl<'d, T: Instance> Fdcan<'d, T, fdcan::ConfigMode> {
             T::regs().txbtie().write(|w| w.0 = 0xffff_ffff);
         }
 
-        can.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo0NewMsg);
-        can.enable_interrupt(fdcan::interrupt::Interrupt::RxFifo1NewMsg);
-        can.enable_interrupt(fdcan::interrupt::Interrupt::TxComplete);
-        can.enable_interrupt_line(fdcan::interrupt::InterruptLine::_0, true);
-        can.enable_interrupt_line(fdcan::interrupt::InterruptLine::_1, true);
+        T::regs().ie().modify(|w| {
+            w.set_rfne(0, true); // Rx Fifo 0 New Msg
+            w.set_rfne(1, true); // Rx Fifo 1 New Msg
+            w.set_tce(true); //  Tx Complete
+        });
+        T::regs().ile().modify(|w| {
+            w.set_eint0(true); // Interrupt Line 0
+            w.set_eint1(true); // Interrupt Line 1
+        });
+
+        Self {
+            config,
+            instance: FdcanInstance(peri),
+        }
+    }
+
+    /// Get configuration
+    pub fn config(&self) -> crate::can::fd::config::FdCanConfig {
+        return self.config;
+    }
 
-        let ns_per_timer_tick = calc_ns_per_timer_tick::<T>(can.get_config().frame_transmit);
-        Self { can, ns_per_timer_tick }
+    /// Set configuration
+    pub fn set_config(&mut self, config: crate::can::fd::config::FdCanConfig) {
+        self.config = config;
     }
 
     /// Configures the bit timings calculated from supplied bitrate.
     pub fn set_bitrate(&mut self, bitrate: u32) {
         let bit_timing = util::calc_can_timings(T::frequency(), bitrate).unwrap();
-        self.can.set_nominal_bit_timing(config::NominalBitTiming {
+
+        let nbtr = crate::can::fd::config::NominalBitTiming {
             sync_jump_width: bit_timing.sync_jump_width,
             prescaler: bit_timing.prescaler,
             seg1: bit_timing.seg1,
             seg2: bit_timing.seg2,
-        });
+        };
+        self.config = self.config.set_nominal_bit_timing(nbtr);
     }
-}
 
-macro_rules! impl_transition {
-    ($from_mode:ident, $to_mode:ident, $name:ident, $func: ident) => {
-        impl<'d, T: Instance> Fdcan<'d, T, fdcan::$from_mode> {
-            /// Transition from $from_mode:ident mode to $to_mode:ident mode
-            pub fn $name(self) -> Fdcan<'d, T, fdcan::$to_mode> {
-                let ns_per_timer_tick = calc_ns_per_timer_tick::<T>(self.can.get_config().frame_transmit);
-                Fdcan {
-                    can: self.can.$func(),
-                    ns_per_timer_tick,
-                }
-            }
+    /// Configures the bit timings for VBR data calculated from supplied bitrate. This also sets confit to allow can FD and VBR
+    pub fn set_fd_data_bitrate(&mut self, bitrate: u32, transceiver_delay_compensation: bool) {
+        let bit_timing = util::calc_can_timings(T::frequency(), bitrate).unwrap();
+        // Note, used existing calcluation for normal(non-VBR) bitrate, appears to work for 250k/1M
+        let nbtr = crate::can::fd::config::DataBitTiming {
+            transceiver_delay_compensation,
+            sync_jump_width: bit_timing.sync_jump_width,
+            prescaler: bit_timing.prescaler,
+            seg1: bit_timing.seg1,
+            seg2: bit_timing.seg2,
+        };
+        self.config.frame_transmit = FrameTransmissionConfig::AllowFdCanAndBRS;
+        self.config = self.config.set_data_bit_timing(nbtr);
+    }
+
+    /// Set an Standard Address CAN filter into slot 'id'
+    #[inline]
+    pub fn set_standard_filter(&mut self, slot: StandardFilterSlot, filter: StandardFilter) {
+        T::registers().msg_ram_mut().filters.flssa[slot as usize].activate(filter);
+    }
+
+    /// Set an array of Standard Address CAN filters and overwrite the current set
+    pub fn set_standard_filters(&mut self, filters: &[StandardFilter; STANDARD_FILTER_MAX as usize]) {
+        for (i, f) in filters.iter().enumerate() {
+            T::registers().msg_ram_mut().filters.flssa[i].activate(*f);
         }
-    };
-}
+    }
 
-impl_transition!(PoweredDownMode, ConfigMode, into_config_mode, into_config_mode);
-impl_transition!(InternalLoopbackMode, ConfigMode, into_config_mode, into_config_mode);
+    /// Set an Extended Address CAN filter into slot 'id'
+    #[inline]
+    pub fn set_extended_filter(&mut self, slot: ExtendedFilterSlot, filter: ExtendedFilter) {
+        T::registers().msg_ram_mut().filters.flesa[slot as usize].activate(filter);
+    }
 
-impl_transition!(ConfigMode, NormalOperationMode, into_normal_mode, into_normal);
-impl_transition!(
-    ConfigMode,
-    ExternalLoopbackMode,
-    into_external_loopback_mode,
-    into_external_loopback
-);
-impl_transition!(
-    ConfigMode,
-    InternalLoopbackMode,
-    into_internal_loopback_mode,
-    into_internal_loopback
-);
+    /// Set an array of Extended Address CAN filters and overwrite the current set
+    pub fn set_extended_filters(&mut self, filters: &[ExtendedFilter; EXTENDED_FILTER_MAX as usize]) {
+        for (i, f) in filters.iter().enumerate() {
+            T::registers().msg_ram_mut().filters.flesa[i].activate(*f);
+        }
+    }
 
-impl<'d, T: Instance, M: FdcanOperatingMode> Fdcan<'d, T, M>
-where
-    M: fdcan::Transmit,
-    M: fdcan::Receive,
-{
-    /// Queues the message to be sent but exerts backpressure.  If a lower-priority
-    /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
-    /// can be replaced, this call asynchronously waits for a frame to be successfully
-    /// transmitted, then tries again.
-    pub async fn write(&mut self, frame: &TxFrame) -> Option<TxFrame> {
-        poll_fn(|cx| {
-            T::state().tx_waker.register(cx.waker());
-            if let Ok(dropped) = self
-                .can
-                .transmit_preserve(frame.header, &frame.data.bytes, &mut |_, hdr, data32| {
-                    TxFrame::from_preserved(hdr, data32)
-                })
-            {
-                return Poll::Ready(dropped.flatten());
-            }
+    /// Start in mode.
+    pub fn start(self, mode: FdcanOperatingMode) -> Fdcan<'d, T> {
+        let ns_per_timer_tick = calc_ns_per_timer_tick::<T>(self.config.frame_transmit);
+        critical_section::with(|_| unsafe {
+            T::mut_state().ns_per_timer_tick = ns_per_timer_tick;
+        });
+        T::registers().into_mode(self.config, mode);
+        let ret = Fdcan {
+            config: self.config,
+            instance: self.instance,
+            _mode: mode,
+        };
+        ret
+    }
 
-            // Couldn't replace any lower priority frames.  Need to wait for some mailboxes
-            // to clear.
-            Poll::Pending
-        })
-        .await
+    /// Start, entering mode. Does same as start(mode)
+    pub fn into_normal_mode(self) -> Fdcan<'d, T> {
+        self.start(FdcanOperatingMode::NormalOperationMode)
     }
 
+    /// Start, entering mode. Does same as start(mode)
+    pub fn into_internal_loopback_mode(self) -> Fdcan<'d, T> {
+        self.start(FdcanOperatingMode::InternalLoopbackMode)
+    }
+
+    /// Start, entering mode. Does same as start(mode)
+    pub fn into_external_loopback_mode(self) -> Fdcan<'d, T> {
+        self.start(FdcanOperatingMode::ExternalLoopbackMode)
+    }
+}
+
+/// FDCAN Instance
+pub struct Fdcan<'d, T: Instance> {
+    config: crate::can::fd::config::FdCanConfig,
+    /// Reference to internals.
+    instance: FdcanInstance<'d, T>,
+    _mode: FdcanOperatingMode,
+}
+
+impl<'d, T: Instance> Fdcan<'d, T> {
     /// Flush one of the TX mailboxes.
-    pub async fn flush(&self, mb: fdcan::Mailbox) {
+    pub async fn flush(&self, idx: usize) {
         poll_fn(|cx| {
-            T::state().tx_waker.register(cx.waker());
+            T::state().tx_mode.register(cx.waker());
 
-            let idx: u8 = mb.into();
+            if idx > 3 {
+                panic!("Bad mailbox");
+            }
             let idx = 1 << idx;
             if !T::regs().txbrp().read().trp(idx) {
                 return Poll::Ready(());
@@ -418,179 +345,454 @@ where
         .await;
     }
 
+    /// Queues the message to be sent but exerts backpressure.  If a lower-priority
+    /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
+    /// can be replaced, this call asynchronously waits for a frame to be successfully
+    /// transmitted, then tries again.
+    pub async fn write(&mut self, frame: &ClassicFrame) -> Option<ClassicFrame> {
+        T::state().tx_mode.write::<T>(frame).await
+    }
+
     /// Returns the next received message frame
-    pub async fn read(&mut self) -> Result<RxFrame, BusError> {
-        poll_fn(|cx| {
-            T::state().err_waker.register(cx.waker());
-            T::state().rx_waker.register(cx.waker());
-
-            let mut buffer: [u8; 64] = [0; 64];
-            if let Ok(rx) = self.can.receive0(&mut buffer) {
-                // rx: fdcan::ReceiveOverrun<RxFrameInfo>
-                // TODO: report overrun?
-                //  for now we just drop it
-
-                let frame: RxFrame = RxFrame::new(
-                    rx.unwrap(),
-                    &buffer,
-                    #[cfg(feature = "time")]
-                    calc_timestamp::<T>(self.ns_per_timer_tick, rx.unwrap().time_stamp),
-                );
-                return Poll::Ready(Ok(frame));
-            } else if let Ok(rx) = self.can.receive1(&mut buffer) {
-                // rx: fdcan::ReceiveOverrun<RxFrameInfo>
-                // TODO: report overrun?
-                //  for now we just drop it
-
-                let frame: RxFrame = RxFrame::new(
-                    rx.unwrap(),
-                    &buffer,
-                    #[cfg(feature = "time")]
-                    calc_timestamp::<T>(self.ns_per_timer_tick, rx.unwrap().time_stamp),
-                );
-                return Poll::Ready(Ok(frame));
-            } else if let Some(err) = curr_error::<T>() {
-                // TODO: this is probably wrong
-                return Poll::Ready(Err(err));
-            }
-            Poll::Pending
-        })
-        .await
+    pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> {
+        T::state().rx_mode.read::<T>().await
+    }
+
+    /// Queues the message to be sent but exerts backpressure.  If a lower-priority
+    /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
+    /// can be replaced, this call asynchronously waits for a frame to be successfully
+    /// transmitted, then tries again.
+    pub async fn write_fd(&mut self, frame: &FdFrame) -> Option<FdFrame> {
+        T::state().tx_mode.write_fd::<T>(frame).await
+    }
+
+    /// Returns the next received message frame
+    pub async fn read_fd(&mut self) -> Result<(FdFrame, Timestamp), BusError> {
+        T::state().rx_mode.read_fd::<T>().await
     }
 
     /// Split instance into separate Tx(write) and Rx(read) portions
-    pub fn split<'c>(&'c mut self) -> (FdcanTx<'c, 'd, T, M>, FdcanRx<'c, 'd, T, M>) {
-        let (mut _control, tx, rx0, rx1) = self.can.split_by_ref();
+    pub fn split(self) -> (FdcanTx<'d, T>, FdcanRx<'d, T>) {
         (
-            FdcanTx { _control, tx },
+            FdcanTx {
+                config: self.config,
+                _instance: self.instance,
+                _mode: self._mode,
+            },
             FdcanRx {
-                rx0,
-                rx1,
-                ns_per_timer_tick: self.ns_per_timer_tick,
+                _instance1: PhantomData::<T>,
+                _instance2: T::regs(),
+                _mode: self._mode,
             },
         )
     }
+
+    /// Join split rx and tx portions back together
+    pub fn join(tx: FdcanTx<'d, T>, rx: FdcanRx<'d, T>) -> Self {
+        Fdcan {
+            config: tx.config,
+            //_instance2: T::regs(),
+            instance: tx._instance,
+            _mode: rx._mode,
+        }
+    }
+
+    /// Return a buffered instance of driver without CAN FD support. User must supply Buffers
+    pub fn buffered<const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize>(
+        &self,
+        tx_buf: &'static mut TxBuf<TX_BUF_SIZE>,
+        rxb: &'static mut RxBuf<RX_BUF_SIZE>,
+    ) -> BufferedCan<'d, T, TX_BUF_SIZE, RX_BUF_SIZE> {
+        BufferedCan::new(PhantomData::<T>, T::regs(), self._mode, tx_buf, rxb)
+    }
+
+    /// Return a buffered instance of driver with CAN FD support. User must supply Buffers
+    pub fn buffered_fd<const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize>(
+        &self,
+        tx_buf: &'static mut TxFdBuf<TX_BUF_SIZE>,
+        rxb: &'static mut RxFdBuf<RX_BUF_SIZE>,
+    ) -> BufferedCanFd<'d, T, TX_BUF_SIZE, RX_BUF_SIZE> {
+        BufferedCanFd::new(PhantomData::<T>, T::regs(), self._mode, tx_buf, rxb)
+    }
 }
 
-/// FDCAN Tx only Instance
-pub struct FdcanTx<'c, 'd, T: Instance, M: fdcan::Transmit> {
-    _control: &'c mut fdcan::FdCanControl<FdcanInstance<'d, T>, M>,
-    tx: &'c mut fdcan::Tx<FdcanInstance<'d, T>, M>,
+/// User supplied buffer for RX Buffering
+pub type RxBuf<const BUF_SIZE: usize> = Channel<CriticalSectionRawMutex, (ClassicFrame, Timestamp), BUF_SIZE>;
+
+/// User supplied buffer for TX buffering
+pub type TxBuf<const BUF_SIZE: usize> = Channel<CriticalSectionRawMutex, ClassicFrame, BUF_SIZE>;
+
+/// Buffered FDCAN Instance
+pub struct BufferedCan<'d, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> {
+    _instance1: PhantomData<T>,
+    _instance2: &'d crate::pac::can::Fdcan,
+    _mode: FdcanOperatingMode,
+    tx_buf: &'static TxBuf<TX_BUF_SIZE>,
+    rx_buf: &'static RxBuf<RX_BUF_SIZE>,
 }
 
-impl<'c, 'd, T: Instance, M: fdcan::Transmit> FdcanTx<'c, 'd, T, M> {
-    /// Queues the message to be sent but exerts backpressure.  If a lower-priority
-    /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
-    /// can be replaced, this call asynchronously waits for a frame to be successfully
-    /// transmitted, then tries again.
-    pub async fn write(&mut self, frame: &TxFrame) -> Option<TxFrame> {
-        poll_fn(|cx| {
-            T::state().tx_waker.register(cx.waker());
-            if let Ok(dropped) = self
-                .tx
-                .transmit_preserve(frame.header, &frame.data.bytes, &mut |_, hdr, data32| {
-                    TxFrame::from_preserved(hdr, data32)
-                })
-            {
-                return Poll::Ready(dropped.flatten());
-            }
+impl<'c, 'd, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize>
+    BufferedCan<'d, T, TX_BUF_SIZE, RX_BUF_SIZE>
+{
+    fn new(
+        _instance1: PhantomData<T>,
+        _instance2: &'d crate::pac::can::Fdcan,
+        _mode: FdcanOperatingMode,
+        tx_buf: &'static TxBuf<TX_BUF_SIZE>,
+        rx_buf: &'static RxBuf<RX_BUF_SIZE>,
+    ) -> Self {
+        BufferedCan {
+            _instance1,
+            _instance2,
+            _mode,
+            tx_buf,
+            rx_buf,
+        }
+        .setup()
+    }
 
-            // Couldn't replace any lower priority frames.  Need to wait for some mailboxes
-            // to clear.
-            Poll::Pending
-        })
-        .await
+    fn setup(self) -> Self {
+        // We don't want interrupts being processed while we change modes.
+        critical_section::with(|_| unsafe {
+            let rx_inner = sealed::ClassicBufferedRxInner {
+                rx_sender: self.rx_buf.sender().into(),
+            };
+            let tx_inner = sealed::ClassicBufferedTxInner {
+                tx_receiver: self.tx_buf.receiver().into(),
+            };
+            T::mut_state().rx_mode = sealed::RxMode::ClassicBuffered(rx_inner);
+            T::mut_state().tx_mode = sealed::TxMode::ClassicBuffered(tx_inner);
+        });
+        self
+    }
+
+    /// Async write frame to TX buffer.
+    pub async fn write(&mut self, frame: ClassicFrame) {
+        self.tx_buf.send(frame).await;
+        T::IT0Interrupt::pend(); // Wake for Tx
+    }
+
+    /// Async read frame from RX buffer.
+    pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> {
+        Ok(self.rx_buf.receive().await)
     }
 }
 
-/// FDCAN Rx only Instance
-#[allow(dead_code)]
-pub struct FdcanRx<'c, 'd, T: Instance, M: fdcan::Receive> {
-    rx0: &'c mut fdcan::Rx<FdcanInstance<'d, T>, M, fdcan::Fifo0>,
-    rx1: &'c mut fdcan::Rx<FdcanInstance<'d, T>, M, fdcan::Fifo1>,
-    ns_per_timer_tick: u64, // For FDCAN internal timer
+impl<'c, 'd, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> Drop
+    for BufferedCan<'d, T, TX_BUF_SIZE, RX_BUF_SIZE>
+{
+    fn drop(&mut self) {
+        critical_section::with(|_| unsafe {
+            T::mut_state().rx_mode = sealed::RxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new());
+            T::mut_state().tx_mode = sealed::TxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new());
+        });
+    }
 }
 
-impl<'c, 'd, T: Instance, M: fdcan::Receive> FdcanRx<'c, 'd, T, M> {
-    /// Returns the next received message frame
-    pub async fn read(&mut self) -> Result<RxFrame, BusError> {
-        poll_fn(|cx| {
-            T::state().err_waker.register(cx.waker());
-            T::state().rx_waker.register(cx.waker());
-
-            let mut buffer: [u8; 64] = [0; 64];
-            if let Ok(rx) = self.rx0.receive(&mut buffer) {
-                // rx: fdcan::ReceiveOverrun<RxFrameInfo>
-                // TODO: report overrun?
-                //  for now we just drop it
-                let frame: RxFrame = RxFrame::new(
-                    rx.unwrap(),
-                    &buffer,
-                    #[cfg(feature = "time")]
-                    calc_timestamp::<T>(self.ns_per_timer_tick, rx.unwrap().time_stamp),
-                );
-                return Poll::Ready(Ok(frame));
-            } else if let Ok(rx) = self.rx1.receive(&mut buffer) {
-                // rx: fdcan::ReceiveOverrun<RxFrameInfo>
-                // TODO: report overrun?
-                //  for now we just drop it
-                let frame: RxFrame = RxFrame::new(
-                    rx.unwrap(),
-                    &buffer,
-                    #[cfg(feature = "time")]
-                    calc_timestamp::<T>(self.ns_per_timer_tick, rx.unwrap().time_stamp),
-                );
-                return Poll::Ready(Ok(frame));
-            } else if let Some(err) = curr_error::<T>() {
-                // TODO: this is probably wrong
-                return Poll::Ready(Err(err));
-            }
+/// User supplied buffer for RX Buffering
+pub type RxFdBuf<const BUF_SIZE: usize> = Channel<CriticalSectionRawMutex, (FdFrame, Timestamp), BUF_SIZE>;
 
-            Poll::Pending
-        })
-        .await
+/// User supplied buffer for TX buffering
+pub type TxFdBuf<const BUF_SIZE: usize> = Channel<CriticalSectionRawMutex, FdFrame, BUF_SIZE>;
+
+/// Buffered FDCAN Instance
+pub struct BufferedCanFd<'d, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> {
+    _instance1: PhantomData<T>,
+    _instance2: &'d crate::pac::can::Fdcan,
+    _mode: FdcanOperatingMode,
+    tx_buf: &'static TxFdBuf<TX_BUF_SIZE>,
+    rx_buf: &'static RxFdBuf<RX_BUF_SIZE>,
+}
+
+impl<'c, 'd, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize>
+    BufferedCanFd<'d, T, TX_BUF_SIZE, RX_BUF_SIZE>
+{
+    fn new(
+        _instance1: PhantomData<T>,
+        _instance2: &'d crate::pac::can::Fdcan,
+        _mode: FdcanOperatingMode,
+        tx_buf: &'static TxFdBuf<TX_BUF_SIZE>,
+        rx_buf: &'static RxFdBuf<RX_BUF_SIZE>,
+    ) -> Self {
+        BufferedCanFd {
+            _instance1,
+            _instance2,
+            _mode,
+            tx_buf,
+            rx_buf,
+        }
+        .setup()
+    }
+
+    fn setup(self) -> Self {
+        // We don't want interrupts being processed while we change modes.
+        critical_section::with(|_| unsafe {
+            let rx_inner = sealed::FdBufferedRxInner {
+                rx_sender: self.rx_buf.sender().into(),
+            };
+            let tx_inner = sealed::FdBufferedTxInner {
+                tx_receiver: self.tx_buf.receiver().into(),
+            };
+            T::mut_state().rx_mode = sealed::RxMode::FdBuffered(rx_inner);
+            T::mut_state().tx_mode = sealed::TxMode::FdBuffered(tx_inner);
+        });
+        self
+    }
+
+    /// Async write frame to TX buffer.
+    pub async fn write(&mut self, frame: FdFrame) {
+        self.tx_buf.send(frame).await;
+        T::IT0Interrupt::pend(); // Wake for Tx
+    }
+
+    /// Async read frame from RX buffer.
+    pub async fn read(&mut self) -> Result<(FdFrame, Timestamp), BusError> {
+        Ok(self.rx_buf.receive().await)
     }
 }
-impl<'d, T: Instance, M: FdcanOperatingMode> Deref for Fdcan<'d, T, M> {
-    type Target = fdcan::FdCan<FdcanInstance<'d, T>, M>;
 
-    fn deref(&self) -> &Self::Target {
-        &self.can
+impl<'c, 'd, T: Instance, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> Drop
+    for BufferedCanFd<'d, T, TX_BUF_SIZE, RX_BUF_SIZE>
+{
+    fn drop(&mut self) {
+        critical_section::with(|_| unsafe {
+            T::mut_state().rx_mode = sealed::RxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new());
+            T::mut_state().tx_mode = sealed::TxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new());
+        });
     }
 }
 
-impl<'d, T: Instance, M: FdcanOperatingMode> DerefMut for Fdcan<'d, T, M> {
-    fn deref_mut(&mut self) -> &mut Self::Target {
-        &mut self.can
+/// FDCAN Rx only Instance
+pub struct FdcanRx<'d, T: Instance> {
+    _instance1: PhantomData<T>,
+    _instance2: &'d crate::pac::can::Fdcan,
+    _mode: FdcanOperatingMode,
+}
+
+/// FDCAN Tx only Instance
+pub struct FdcanTx<'d, T: Instance> {
+    config: crate::can::fd::config::FdCanConfig,
+    _instance: FdcanInstance<'d, T>, //(PeripheralRef<'a, T>);
+    _mode: FdcanOperatingMode,
+}
+
+impl<'c, 'd, T: Instance> FdcanTx<'d, T> {
+    /// Queues the message to be sent but exerts backpressure.  If a lower-priority
+    /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
+    /// can be replaced, this call asynchronously waits for a frame to be successfully
+    /// transmitted, then tries again.
+    pub async fn write(&mut self, frame: &ClassicFrame) -> Option<ClassicFrame> {
+        T::state().tx_mode.write::<T>(frame).await
+    }
+
+    /// Queues the message to be sent but exerts backpressure.  If a lower-priority
+    /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
+    /// can be replaced, this call asynchronously waits for a frame to be successfully
+    /// transmitted, then tries again.
+    pub async fn write_fd(&mut self, frame: &FdFrame) -> Option<FdFrame> {
+        T::state().tx_mode.write_fd::<T>(frame).await
+    }
+}
+
+impl<'c, 'd, T: Instance> FdcanRx<'d, T> {
+    /// Returns the next received message frame
+    pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> {
+        T::state().rx_mode.read::<T>().await
+    }
+
+    /// Returns the next received message frame
+    pub async fn read_fd(&mut self) -> Result<(FdFrame, Timestamp), BusError> {
+        T::state().rx_mode.read_fd::<T>().await
     }
 }
 
 pub(crate) mod sealed {
+    use core::future::poll_fn;
+    use core::task::Poll;
+
+    use embassy_sync::channel::{DynamicReceiver, DynamicSender};
     use embassy_sync::waitqueue::AtomicWaker;
 
+    use crate::can::_version::{BusError, Timestamp};
+    use crate::can::frame::{ClassicFrame, FdFrame};
+
+    pub struct ClassicBufferedRxInner {
+        pub rx_sender: DynamicSender<'static, (ClassicFrame, Timestamp)>,
+    }
+    pub struct ClassicBufferedTxInner {
+        pub tx_receiver: DynamicReceiver<'static, ClassicFrame>,
+    }
+
+    pub struct FdBufferedRxInner {
+        pub rx_sender: DynamicSender<'static, (FdFrame, Timestamp)>,
+    }
+    pub struct FdBufferedTxInner {
+        pub tx_receiver: DynamicReceiver<'static, FdFrame>,
+    }
+
+    pub enum RxMode {
+        NonBuffered(AtomicWaker),
+        ClassicBuffered(ClassicBufferedRxInner),
+        FdBuffered(FdBufferedRxInner),
+    }
+
+    impl RxMode {
+        pub fn register(&self, arg: &core::task::Waker) {
+            match self {
+                RxMode::NonBuffered(waker) => waker.register(arg),
+                _ => {
+                    panic!("Bad Mode")
+                }
+            }
+        }
+
+        pub fn on_interrupt<T: Instance>(&self, fifonr: usize) {
+            T::regs().ir().write(|w| w.set_rfn(fifonr, true));
+            match self {
+                RxMode::NonBuffered(waker) => {
+                    waker.wake();
+                }
+                RxMode::ClassicBuffered(buf) => {
+                    if let Some(r) = T::registers().read_classic(fifonr) {
+                        let ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1);
+                        let _ = buf.rx_sender.try_send((r.0, ts));
+                    }
+                }
+                RxMode::FdBuffered(buf) => {
+                    if let Some(r) = T::registers().read_fd(fifonr) {
+                        let ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1);
+                        let _ = buf.rx_sender.try_send((r.0, ts));
+                    }
+                }
+            }
+        }
+
+        pub async fn read<T: Instance>(&self) -> Result<(ClassicFrame, Timestamp), BusError> {
+            poll_fn(|cx| {
+                T::state().err_waker.register(cx.waker());
+                self.register(cx.waker());
+
+                if let Some((msg, ts)) = T::registers().read_classic(0) {
+                    let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts);
+                    return Poll::Ready(Ok((msg, ts)));
+                } else if let Some((msg, ts)) = T::registers().read_classic(1) {
+                    let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts);
+                    return Poll::Ready(Ok((msg, ts)));
+                } else if let Some(err) = T::registers().curr_error() {
+                    // TODO: this is probably wrong
+                    return Poll::Ready(Err(err));
+                }
+                Poll::Pending
+            })
+            .await
+        }
+
+        pub async fn read_fd<T: Instance>(&self) -> Result<(FdFrame, Timestamp), BusError> {
+            poll_fn(|cx| {
+                T::state().err_waker.register(cx.waker());
+                self.register(cx.waker());
+
+                if let Some((msg, ts)) = T::registers().read_fd(0) {
+                    let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts);
+                    return Poll::Ready(Ok((msg, ts)));
+                } else if let Some((msg, ts)) = T::registers().read_fd(1) {
+                    let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts);
+                    return Poll::Ready(Ok((msg, ts)));
+                } else if let Some(err) = T::registers().curr_error() {
+                    // TODO: this is probably wrong
+                    return Poll::Ready(Err(err));
+                }
+                Poll::Pending
+            })
+            .await
+        }
+    }
+
+    pub enum TxMode {
+        NonBuffered(AtomicWaker),
+        ClassicBuffered(ClassicBufferedTxInner),
+        FdBuffered(FdBufferedTxInner),
+    }
+
+    impl TxMode {
+        pub fn register(&self, arg: &core::task::Waker) {
+            match self {
+                TxMode::NonBuffered(waker) => {
+                    waker.register(arg);
+                }
+                _ => {
+                    panic!("Bad mode");
+                }
+            }
+        }
+
+        /// Queues the message to be sent but exerts backpressure.  If a lower-priority
+        /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
+        /// can be replaced, this call asynchronously waits for a frame to be successfully
+        /// transmitted, then tries again.
+        pub async fn write<T: Instance>(&self, frame: &ClassicFrame) -> Option<ClassicFrame> {
+            poll_fn(|cx| {
+                self.register(cx.waker());
+
+                if let Ok(dropped) = T::registers().write_classic(frame) {
+                    return Poll::Ready(dropped);
+                }
+
+                // Couldn't replace any lower priority frames.  Need to wait for some mailboxes
+                // to clear.
+                Poll::Pending
+            })
+            .await
+        }
+
+        /// Queues the message to be sent but exerts backpressure.  If a lower-priority
+        /// frame is dropped from the mailbox, it is returned.  If no lower-priority frames
+        /// can be replaced, this call asynchronously waits for a frame to be successfully
+        /// transmitted, then tries again.
+        pub async fn write_fd<T: Instance>(&self, frame: &FdFrame) -> Option<FdFrame> {
+            poll_fn(|cx| {
+                self.register(cx.waker());
+
+                if let Ok(dropped) = T::registers().write_fd(frame) {
+                    return Poll::Ready(dropped);
+                }
+
+                // Couldn't replace any lower priority frames.  Need to wait for some mailboxes
+                // to clear.
+                Poll::Pending
+            })
+            .await
+        }
+    }
+
     pub struct State {
-        pub tx_waker: AtomicWaker,
+        pub rx_mode: RxMode,
+        pub tx_mode: TxMode,
+        pub ns_per_timer_tick: u64,
+
         pub err_waker: AtomicWaker,
-        pub rx_waker: AtomicWaker,
     }
 
     impl State {
         pub const fn new() -> Self {
             Self {
-                tx_waker: AtomicWaker::new(),
+                rx_mode: RxMode::NonBuffered(AtomicWaker::new()),
+                tx_mode: TxMode::NonBuffered(AtomicWaker::new()),
+                ns_per_timer_tick: 0,
                 err_waker: AtomicWaker::new(),
-                rx_waker: AtomicWaker::new(),
             }
         }
     }
 
     pub trait Instance {
-        const REGISTERS: *mut fdcan::RegisterBlock;
-        const MSG_RAM: *mut fdcan::message_ram::RegisterBlock;
         const MSG_RAM_OFFSET: usize;
 
         fn regs() -> &'static crate::pac::can::Fdcan;
+        fn registers() -> crate::can::fd::peripheral::Registers;
+        fn ram() -> &'static crate::pac::fdcanram::Fdcanram;
         fn state() -> &'static State;
+        unsafe fn mut_state() -> &'static mut State;
+        fn calc_timestamp(ns_per_timer_tick: u64, ts_val: u16) -> Timestamp;
 
         #[cfg(not(stm32h7))]
         fn configure_msg_ram() {}
@@ -599,7 +801,8 @@ pub(crate) mod sealed {
         fn configure_msg_ram() {
             let r = Self::regs();
 
-            use fdcan::message_ram::*;
+            use crate::can::fd::message_ram::*;
+            //use fdcan::message_ram::*;
             let mut offset_words = Self::MSG_RAM_OFFSET as u16;
 
             // 11-bit filter
@@ -677,32 +880,45 @@ pub trait Instance: sealed::Instance + RccPeripheral + InterruptableInstance + '
 /// Fdcan Instance struct
 pub struct FdcanInstance<'a, T>(PeripheralRef<'a, T>);
 
-unsafe impl<'d, T: Instance> fdcan::message_ram::Instance for FdcanInstance<'d, T> {
-    const MSG_RAM: *mut RegisterBlock = T::MSG_RAM;
-}
-
-unsafe impl<'d, T: Instance> fdcan::Instance for FdcanInstance<'d, T>
-where
-    FdcanInstance<'d, T>: fdcan::message_ram::Instance,
-{
-    const REGISTERS: *mut fdcan::RegisterBlock = T::REGISTERS;
-}
-
 macro_rules! impl_fdcan {
     ($inst:ident, $msg_ram_inst:ident, $msg_ram_offset:literal) => {
         impl sealed::Instance for peripherals::$inst {
-            const REGISTERS: *mut fdcan::RegisterBlock = crate::pac::$inst.as_ptr() as *mut _;
-            const MSG_RAM: *mut fdcan::message_ram::RegisterBlock = crate::pac::$msg_ram_inst.as_ptr() as *mut _;
             const MSG_RAM_OFFSET: usize = $msg_ram_offset;
 
             fn regs() -> &'static crate::pac::can::Fdcan {
                 &crate::pac::$inst
             }
-
+            fn registers() -> Registers {
+                Registers{regs: &crate::pac::$inst, msgram: &crate::pac::$msg_ram_inst}
+            }
+            fn ram() -> &'static crate::pac::fdcanram::Fdcanram {
+                &crate::pac::$msg_ram_inst
+            }
+            unsafe fn mut_state() -> & 'static mut sealed::State {
+                static mut STATE: sealed::State = sealed::State::new();
+                & mut STATE
+            }
             fn state() -> &'static sealed::State {
-                static STATE: sealed::State = sealed::State::new();
-                &STATE
+                unsafe { peripherals::$inst::mut_state() }
             }
+
+#[cfg(feature = "time")]
+fn calc_timestamp(ns_per_timer_tick: u64, ts_val: u16) -> Timestamp {
+    let now_embassy = embassy_time::Instant::now();
+    if ns_per_timer_tick == 0 {
+        return now_embassy;
+    }
+    let cantime = { Self::regs().tscv().read().tsc() };
+    let delta = cantime.overflowing_sub(ts_val).0 as u64;
+    let ns = ns_per_timer_tick * delta as u64;
+    now_embassy - embassy_time::Duration::from_nanos(ns)
+}
+
+#[cfg(not(feature = "time"))]
+fn calc_timestamp(_ns_per_timer_tick: u64, ts_val: u16) -> Timestamp {
+    ts_val
+}
+
         }
 
         impl Instance for peripherals::$inst {}
diff --git a/embassy-stm32/src/can/frame.rs b/embassy-stm32/src/can/frame.rs
new file mode 100644
index 000000000..725a9b1ab
--- /dev/null
+++ b/embassy-stm32/src/can/frame.rs
@@ -0,0 +1,370 @@
+//! Definition for CAN Frames
+use bit_field::BitField;
+
+/// CAN Header, without meta data
+#[derive(Debug, Copy, Clone)]
+pub struct Header {
+    id: embedded_can::Id,
+    len: u8,
+    flags: u8,
+}
+
+impl Header {
+    const FLAG_RTR: usize = 0; // Remote
+    const FLAG_FDCAN: usize = 1; // FDCan vs Classic CAN
+    const FLAG_BRS: usize = 2; // Bit-rate switching, ignored for Classic CAN
+
+    /// Create new CAN Header
+    pub fn new(id: embedded_can::Id, len: u8, rtr: bool) -> Header {
+        let mut flags = 0u8;
+        flags.set_bit(Self::FLAG_RTR, rtr);
+        Header { id, len, flags }
+    }
+
+    /// Create new CAN FD Header
+    pub fn new_fd(id: embedded_can::Id, len: u8, rtr: bool, brs: bool) -> Header {
+        let mut flags = 0u8;
+        flags.set_bit(Self::FLAG_RTR, rtr);
+        flags.set_bit(Self::FLAG_FDCAN, true);
+        flags.set_bit(Self::FLAG_BRS, brs);
+        Header { id, len, flags }
+    }
+
+    /// Return ID
+    pub fn id(&self) -> &embedded_can::Id {
+        &self.id
+    }
+
+    /// Return length as u8
+    pub fn len(&self) -> u8 {
+        self.len
+    }
+
+    /// Is remote frame
+    pub fn rtr(&self) -> bool {
+        self.flags.get_bit(Self::FLAG_RTR)
+    }
+
+    /// Request/is FDCAN frame
+    pub fn fdcan(&self) -> bool {
+        self.flags.get_bit(Self::FLAG_FDCAN)
+    }
+
+    /// Request/is Flexible Data Rate
+    pub fn bit_rate_switching(&self) -> bool {
+        self.flags.get_bit(Self::FLAG_BRS)
+    }
+}
+
+/// Payload of a classic CAN data frame.
+///
+/// Contains 0 to 8 Bytes of data.
+#[derive(Debug, Copy, Clone)]
+pub struct ClassicData {
+    pub(crate) bytes: [u8; 8],
+}
+
+impl ClassicData {
+    /// Creates a data payload from a raw byte slice.
+    ///
+    /// Returns `None` if `data` is more than 64 bytes (which is the maximum) or
+    /// cannot be represented with an FDCAN DLC.
+    pub fn new(data: &[u8]) -> Option<Self> {
+        if !FdData::is_valid_len(data.len()) {
+            return None;
+        }
+
+        let mut bytes = [0; 8];
+        bytes[..data.len()].copy_from_slice(data);
+
+        Some(Self { bytes })
+    }
+
+    /// Raw read access to data.
+    pub fn raw(&self) -> &[u8] {
+        &self.bytes
+    }
+
+    /// Checks if the length can be encoded in FDCAN DLC field.
+    pub const fn is_valid_len(len: usize) -> bool {
+        match len {
+            0..=8 => true,
+            _ => false,
+        }
+    }
+
+    /// Creates an empty data payload containing 0 bytes.
+    #[inline]
+    pub const fn empty() -> Self {
+        Self { bytes: [0; 8] }
+    }
+}
+
+/// Frame with up to 8 bytes of data payload as per Classic CAN
+#[derive(Debug, Copy, Clone)]
+pub struct ClassicFrame {
+    can_header: Header,
+    data: ClassicData,
+}
+
+impl ClassicFrame {
+    pub(crate) const MAX_DATA_LEN: usize = 8;
+
+    /// Create a new CAN classic Frame
+    pub fn new(can_header: Header, data: ClassicData) -> ClassicFrame {
+        ClassicFrame { can_header, data }
+    }
+
+    /// Create new extended frame
+    pub fn new_extended(raw_id: u32, raw_data: &[u8]) -> Option<Self> {
+        if let Some(id) = embedded_can::ExtendedId::new(raw_id) {
+            match ClassicData::new(raw_data) {
+                Some(data) => Some(ClassicFrame::new(
+                    Header::new(id.into(), raw_data.len() as u8, false),
+                    data,
+                )),
+                None => None,
+            }
+        } else {
+            None
+        }
+    }
+
+    /// Create new standard frame
+    pub fn new_standard(raw_id: u16, raw_data: &[u8]) -> Option<Self> {
+        if let Some(id) = embedded_can::StandardId::new(raw_id) {
+            match ClassicData::new(raw_data) {
+                Some(data) => Some(ClassicFrame::new(
+                    Header::new(id.into(), raw_data.len() as u8, false),
+                    data,
+                )),
+                None => None,
+            }
+        } else {
+            None
+        }
+    }
+
+    /// Create new remote frame
+    pub fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Option<Self> {
+        if len <= 8usize {
+            Some(ClassicFrame::new(
+                Header::new(id.into(), len as u8, true),
+                ClassicData::empty(),
+            ))
+        } else {
+            None
+        }
+    }
+
+    /// Get reference to data
+    pub fn header(&self) -> &Header {
+        &self.can_header
+    }
+
+    /// Return ID
+    pub fn id(&self) -> &embedded_can::Id {
+        &self.can_header.id
+    }
+
+    /// Get reference to data
+    pub fn data(&self) -> &[u8] {
+        &self.data.raw()
+    }
+}
+
+impl embedded_can::Frame for ClassicFrame {
+    fn new(id: impl Into<embedded_can::Id>, raw_data: &[u8]) -> Option<Self> {
+        match ClassicData::new(raw_data) {
+            Some(data) => Some(ClassicFrame::new(
+                Header::new(id.into(), raw_data.len() as u8, false),
+                data,
+            )),
+            None => None,
+        }
+    }
+    fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Option<Self> {
+        if len <= 8 {
+            Some(ClassicFrame::new(
+                Header::new(id.into(), len as u8, true),
+                ClassicData::empty(),
+            ))
+        } else {
+            None
+        }
+    }
+    fn is_extended(&self) -> bool {
+        match self.can_header.id {
+            embedded_can::Id::Extended(_) => true,
+            embedded_can::Id::Standard(_) => true,
+        }
+    }
+    fn is_remote_frame(&self) -> bool {
+        self.can_header.rtr()
+    }
+    fn id(&self) -> embedded_can::Id {
+        self.can_header.id
+    }
+    fn dlc(&self) -> usize {
+        self.can_header.len as usize
+    }
+    fn data(&self) -> &[u8] {
+        &self.data.raw()
+    }
+}
+
+/// Payload of a (FD)CAN data frame.
+///
+/// Contains 0 to 64 Bytes of data.
+#[derive(Debug, Copy, Clone)]
+pub struct FdData {
+    pub(crate) bytes: [u8; 64],
+}
+
+impl FdData {
+    /// Creates a data payload from a raw byte slice.
+    ///
+    /// Returns `None` if `data` is more than 64 bytes (which is the maximum) or
+    /// cannot be represented with an FDCAN DLC.
+    pub fn new(data: &[u8]) -> Option<Self> {
+        if !FdData::is_valid_len(data.len()) {
+            return None;
+        }
+
+        let mut bytes = [0; 64];
+        bytes[..data.len()].copy_from_slice(data);
+
+        Some(Self { bytes })
+    }
+
+    /// Raw read access to data.
+    pub fn raw(&self) -> &[u8] {
+        &self.bytes
+    }
+
+    /// Checks if the length can be encoded in FDCAN DLC field.
+    pub const fn is_valid_len(len: usize) -> bool {
+        match len {
+            0..=8 => true,
+            12 => true,
+            16 => true,
+            20 => true,
+            24 => true,
+            32 => true,
+            48 => true,
+            64 => true,
+            _ => false,
+        }
+    }
+
+    /// Creates an empty data payload containing 0 bytes.
+    #[inline]
+    pub const fn empty() -> Self {
+        Self { bytes: [0; 64] }
+    }
+}
+
+/// Frame with up to 8 bytes of data payload as per Fd CAN
+#[derive(Debug, Copy, Clone)]
+pub struct FdFrame {
+    can_header: Header,
+    data: FdData,
+}
+
+impl FdFrame {
+    pub(crate) const MAX_DATA_LEN: usize = 64;
+
+    /// Create a new CAN classic Frame
+    pub fn new(can_header: Header, data: FdData) -> FdFrame {
+        FdFrame { can_header, data }
+    }
+
+    /// Create new extended frame
+    pub fn new_extended(raw_id: u32, raw_data: &[u8]) -> Option<Self> {
+        if let Some(id) = embedded_can::ExtendedId::new(raw_id) {
+            match FdData::new(raw_data) {
+                Some(data) => Some(FdFrame::new(Header::new(id.into(), raw_data.len() as u8, false), data)),
+                None => None,
+            }
+        } else {
+            None
+        }
+    }
+
+    /// Create new standard frame
+    pub fn new_standard(raw_id: u16, raw_data: &[u8]) -> Option<Self> {
+        if let Some(id) = embedded_can::StandardId::new(raw_id) {
+            match FdData::new(raw_data) {
+                Some(data) => Some(FdFrame::new(Header::new(id.into(), raw_data.len() as u8, false), data)),
+                None => None,
+            }
+        } else {
+            None
+        }
+    }
+
+    /// Create new remote frame
+    pub fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Option<Self> {
+        if len <= 8 {
+            Some(FdFrame::new(Header::new(id.into(), len as u8, true), FdData::empty()))
+        } else {
+            None
+        }
+    }
+
+    /// Get reference to data
+    pub fn header(&self) -> &Header {
+        &self.can_header
+    }
+
+    /// Return ID
+    pub fn id(&self) -> &embedded_can::Id {
+        &self.can_header.id
+    }
+
+    /// Get reference to data
+    pub fn data(&self) -> &[u8] {
+        &self.data.raw()
+    }
+}
+
+impl embedded_can::Frame for FdFrame {
+    fn new(id: impl Into<embedded_can::Id>, raw_data: &[u8]) -> Option<Self> {
+        match FdData::new(raw_data) {
+            Some(data) => Some(FdFrame::new(
+                Header::new_fd(id.into(), raw_data.len() as u8, false, true),
+                data,
+            )),
+            None => None,
+        }
+    }
+    fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Option<Self> {
+        if len <= 8 {
+            Some(FdFrame::new(
+                Header::new_fd(id.into(), len as u8, true, true),
+                FdData::empty(),
+            ))
+        } else {
+            None
+        }
+    }
+    fn is_extended(&self) -> bool {
+        match self.can_header.id {
+            embedded_can::Id::Extended(_) => true,
+            embedded_can::Id::Standard(_) => true,
+        }
+    }
+    fn is_remote_frame(&self) -> bool {
+        self.can_header.rtr()
+    }
+    fn id(&self) -> embedded_can::Id {
+        self.can_header.id
+    }
+    // Returns length in bytes even for CANFD packets which embedded-can does not really mention.
+    fn dlc(&self) -> usize {
+        self.can_header.len as usize
+    }
+    fn data(&self) -> &[u8] {
+        &self.data.raw()
+    }
+}
diff --git a/embassy-stm32/src/dma/bdma.rs b/embassy-stm32/src/dma/bdma.rs
index 077cfdcd9..994bdb1e6 100644
--- a/embassy-stm32/src/dma/bdma.rs
+++ b/embassy-stm32/src/dma/bdma.rs
@@ -299,7 +299,7 @@ impl<'a, C: Channel> Transfer<'a, C> {
         STATE.complete_count[this.channel.index()].store(0, Ordering::Release);
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, _request);
+        super::dmamux::configure_dmamux(&*this.channel, _request);
 
         ch.par().write_value(peri_addr as u32);
         ch.mar().write_value(mem_addr as u32);
@@ -483,7 +483,7 @@ impl<'a, C: Channel, W: Word> ReadableRingBuffer<'a, C, W> {
         this.clear_irqs();
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, _request);
+        super::dmamux::configure_dmamux(&*this.channel, _request);
 
         let ch = dma.ch(channel_number);
         ch.par().write_value(peri_addr as u32);
@@ -641,7 +641,7 @@ impl<'a, C: Channel, W: Word> WritableRingBuffer<'a, C, W> {
         this.clear_irqs();
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, _request);
+        super::dmamux::configure_dmamux(&*this.channel, _request);
 
         let ch = dma.ch(channel_number);
         ch.par().write_value(peri_addr as u32);
diff --git a/embassy-stm32/src/dma/dma.rs b/embassy-stm32/src/dma/dma.rs
index ef9bb3d78..e762b1bde 100644
--- a/embassy-stm32/src/dma/dma.rs
+++ b/embassy-stm32/src/dma/dma.rs
@@ -366,7 +366,7 @@ impl<'a, C: Channel> Transfer<'a, C> {
         this.clear_irqs();
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, _request);
+        super::dmamux::configure_dmamux(&*this.channel, _request);
 
         ch.par().write_value(peri_addr as u32);
         ch.m0ar().write_value(mem_addr as u32);
@@ -522,7 +522,7 @@ impl<'a, C: Channel, W: Word> DoubleBuffered<'a, C, W> {
         this.clear_irqs();
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, _request);
+        super::dmamux::configure_dmamux(&*this.channel, _request);
 
         let ch = dma.st(channel_number);
         ch.par().write_value(peri_addr as u32);
@@ -726,7 +726,7 @@ impl<'a, C: Channel, W: Word> ReadableRingBuffer<'a, C, W> {
         this.clear_irqs();
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, _request);
+        super::dmamux::configure_dmamux(&*this.channel, _request);
 
         let ch = dma.st(channel_number);
         ch.par().write_value(peri_addr as u32);
@@ -901,7 +901,7 @@ impl<'a, C: Channel, W: Word> WritableRingBuffer<'a, C, W> {
         this.clear_irqs();
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, _request);
+        super::dmamux::configure_dmamux(&*this.channel, _request);
 
         let ch = dma.st(channel_number);
         ch.par().write_value(peri_addr as u32);
diff --git a/embassy-stm32/src/dma/dmamux.rs b/embassy-stm32/src/dma/dmamux.rs
index 9cd494724..ac6f44107 100644
--- a/embassy-stm32/src/dma/dmamux.rs
+++ b/embassy-stm32/src/dma/dmamux.rs
@@ -2,7 +2,7 @@
 
 use crate::{pac, peripherals};
 
-pub(crate) fn configure_dmamux<M: MuxChannel>(channel: &mut M, request: u8) {
+pub(crate) fn configure_dmamux<M: MuxChannel>(channel: &M, request: u8) {
     let ch_mux_regs = channel.mux_regs().ccr(channel.mux_num());
     ch_mux_regs.write(|reg| {
         reg.set_nbreq(0);
diff --git a/embassy-stm32/src/dma/gpdma.rs b/embassy-stm32/src/dma/gpdma.rs
index 34b2426b9..337e7b309 100644
--- a/embassy-stm32/src/dma/gpdma.rs
+++ b/embassy-stm32/src/dma/gpdma.rs
@@ -259,7 +259,7 @@ impl<'a, C: Channel> Transfer<'a, C> {
         let this = Self { channel };
 
         #[cfg(dmamux)]
-        super::dmamux::configure_dmamux(&mut *this.channel, request);
+        super::dmamux::configure_dmamux(&*this.channel, request);
 
         ch.cr().write(|w| w.set_reset(true));
         ch.fcr().write(|w| w.0 = 0xFFFF_FFFF); // clear all irqs
diff --git a/embassy-stm32/src/flash/h50.rs b/embassy-stm32/src/flash/h50.rs
new file mode 100644
index 000000000..db05bef5d
--- /dev/null
+++ b/embassy-stm32/src/flash/h50.rs
@@ -0,0 +1,124 @@
+/// STM32H50 series flash impl. See RM0492
+use core::{
+    ptr::write_volatile,
+    sync::atomic::{fence, Ordering},
+};
+
+use cortex_m::interrupt;
+use pac::flash::regs::Nssr;
+use pac::flash::vals::Bksel;
+
+use super::{Error, FlashBank, FlashRegion, FlashSector, FLASH_REGIONS, WRITE_SIZE};
+use crate::pac;
+
+pub(crate) const fn is_default_layout() -> bool {
+    true
+}
+
+pub(crate) const fn get_flash_regions() -> &'static [&'static FlashRegion] {
+    &FLASH_REGIONS
+}
+
+pub(crate) unsafe fn lock() {
+    pac::FLASH.nscr().modify(|w| w.set_lock(true));
+}
+
+pub(crate) unsafe fn unlock() {
+    while busy() {}
+
+    if pac::FLASH.nscr().read().lock() {
+        pac::FLASH.nskeyr().write_value(0x4567_0123);
+        pac::FLASH.nskeyr().write_value(0xCDEF_89AB);
+    }
+}
+
+pub(crate) unsafe fn enable_blocking_write() {
+    assert_eq!(0, WRITE_SIZE % 4);
+    pac::FLASH.nscr().write(|w| w.set_pg(true));
+}
+
+pub(crate) unsafe fn disable_blocking_write() {
+    pac::FLASH.nscr().write(|w| w.set_pg(false));
+}
+
+pub(crate) unsafe fn blocking_write(start_address: u32, buf: &[u8; WRITE_SIZE]) -> Result<(), Error> {
+    let mut address = start_address;
+    for val in buf.chunks(4) {
+        write_volatile(address as *mut u32, u32::from_le_bytes(val.try_into().unwrap()));
+        address += val.len() as u32;
+
+        // prevents parallelism errors
+        fence(Ordering::SeqCst);
+    }
+
+    wait_ready_blocking()
+}
+
+pub(crate) unsafe fn blocking_erase_sector(sector: &FlashSector) -> Result<(), Error> {
+    assert!(sector.bank != FlashBank::Otp);
+    assert!(sector.index_in_bank < 8);
+
+    while busy() {}
+
+    interrupt::free(|_| {
+        pac::FLASH.nscr().modify(|w| {
+            w.set_bksel(match sector.bank {
+                FlashBank::Bank1 => Bksel::B_0X0,
+                FlashBank::Bank2 => Bksel::B_0X1,
+                _ => unreachable!(),
+            });
+            w.set_snb(sector.index_in_bank);
+            w.set_ser(true);
+            w.set_strt(true);
+        })
+    });
+
+    let ret = wait_ready_blocking();
+    pac::FLASH.nscr().modify(|w| w.set_ser(false));
+    ret
+}
+
+pub(crate) unsafe fn wait_ready_blocking() -> Result<(), Error> {
+    loop {
+        let sr = pac::FLASH.nssr().read();
+
+        if !sr_busy(sr) {
+            if sr.wrperr() {
+                return Err(Error::Protected);
+            }
+            if sr.pgserr() {
+                return Err(Error::Seq);
+            }
+            if sr.strberr() {
+                // writing several times to the same byte in the write buffer
+                return Err(Error::Prog);
+            }
+            if sr.incerr() {
+                // attempting write operation before completion of previous
+                // write operation
+                return Err(Error::Seq);
+            }
+
+            return Ok(());
+        }
+    }
+}
+
+pub(crate) unsafe fn clear_all_err() {
+    pac::FLASH.nsccr().modify(|w| {
+        w.set_clr_wrperr(true);
+        w.set_clr_pgserr(true);
+        w.set_clr_strberr(true);
+        w.set_clr_incerr(true);
+    })
+}
+
+fn sr_busy(sr: Nssr) -> bool {
+    // Note: RM0492 sometimes incorrectly refers to WBNE as NSWBNE
+    sr.bsy() || sr.dbne() || sr.wbne()
+}
+
+fn busy() -> bool {
+    let sr = pac::FLASH.nssr().read();
+    sr_busy(sr)
+}
diff --git a/embassy-stm32/src/flash/mod.rs b/embassy-stm32/src/flash/mod.rs
index 4f43a7a48..1d8031e82 100644
--- a/embassy-stm32/src/flash/mod.rs
+++ b/embassy-stm32/src/flash/mod.rs
@@ -102,10 +102,11 @@ pub enum FlashBank {
 #[cfg_attr(flash_h7, path = "h7.rs")]
 #[cfg_attr(flash_h7ab, path = "h7.rs")]
 #[cfg_attr(flash_u5, path = "u5.rs")]
+#[cfg_attr(flash_h50, path = "h50.rs")]
 #[cfg_attr(
     not(any(
         flash_l0, flash_l1, flash_l4, flash_wl, flash_wb, flash_f0, flash_f1, flash_f3, flash_f4, flash_f7, flash_g0,
-        flash_g4, flash_h7, flash_h7ab, flash_u5
+        flash_g4, flash_h7, flash_h7ab, flash_u5, flash_h50
     )),
     path = "other.rs"
 )]
diff --git a/embassy-stm32/src/rcc/bd.rs b/embassy-stm32/src/rcc/bd.rs
index d381ec1bd..39407b28c 100644
--- a/embassy-stm32/src/rcc/bd.rs
+++ b/embassy-stm32/src/rcc/bd.rs
@@ -201,11 +201,18 @@ impl LsConfig {
             bdcr().modify(|w| w.set_bdrst(true));
             bdcr().modify(|w| w.set_bdrst(false));
         }
-        #[cfg(any(stm32h5))]
-        {
-            bdcr().modify(|w| w.set_vswrst(true));
-            bdcr().modify(|w| w.set_vswrst(false));
-        }
+        // H5 has a terrible, terrible errata: 'SRAM2 is erased when the backup domain is reset'
+        // pending a more sane sane way to handle this, just don't reset BD for now.
+        // This means the RTCSEL write below will have no effect, only if it has already been written
+        // after last power-on. Since it's uncommon to dynamically change RTCSEL, this is better than
+        // letting half our RAM go magically *poof*.
+        // STM32H503CB/EB/KB/RB device errata - 2.2.8 SRAM2 unduly erased upon a backup domain reset
+        // STM32H562xx/563xx/573xx device errata - 2.2.14 SRAM2 is erased when the backup domain is reset
+        //#[cfg(any(stm32h5))]
+        //{
+        //    bdcr().modify(|w| w.set_vswrst(true));
+        //    bdcr().modify(|w| w.set_vswrst(false));
+        //}
         #[cfg(any(stm32c0, stm32l0))]
         {
             bdcr().modify(|w| w.set_rtcrst(true));
diff --git a/embassy-stm32/src/rcc/u5.rs b/embassy-stm32/src/rcc/u5.rs
index 20cc3112a..72613f0f3 100644
--- a/embassy-stm32/src/rcc/u5.rs
+++ b/embassy-stm32/src/rcc/u5.rs
@@ -1,134 +1,83 @@
-pub use crate::pac::rcc::vals::{Hpre as AHBPrescaler, Msirange, Plldiv, Pllm, Plln, Ppre as APBPrescaler};
-use crate::pac::rcc::vals::{Msirgsel, Pllmboost, Pllrge, Pllsrc, Sw};
+pub use crate::pac::pwr::vals::Vos as VoltageScale;
+pub use crate::pac::rcc::vals::{
+    Hpre as AHBPrescaler, Msirange, Msirange as MSIRange, Plldiv as PllDiv, Pllm as PllPreDiv, Plln as PllMul,
+    Pllsrc as PllSource, Ppre as APBPrescaler, Sw as ClockSrc,
+};
+use crate::pac::rcc::vals::{Hseext, Msirgsel, Pllmboost, Pllrge};
 use crate::pac::{FLASH, PWR, RCC};
 use crate::time::Hertz;
 
 /// HSI speed
 pub const HSI_FREQ: Hertz = Hertz(16_000_000);
 
-pub use crate::pac::pwr::vals::Vos as VoltageScale;
-
-#[derive(Copy, Clone)]
-#[allow(non_camel_case_types)]
-pub enum ClockSrc {
-    /// Use an internal medium speed oscillator (MSIS) as the system clock.
-    MSI(Msirange),
-    /// Use the external high speed clock as the system clock.
-    ///
-    /// HSE clocks faster than 25 MHz require at least `VoltageScale::RANGE3`, and HSE clocks must
-    /// never exceed 50 MHz.
-    HSE(Hertz),
-    /// Use the 16 MHz internal high speed oscillator as the system clock.
-    HSI,
-    /// Use PLL1 as the system clock.
-    PLL1_R(PllConfig),
+#[derive(Clone, Copy, Eq, PartialEq)]
+pub enum HseMode {
+    /// crystal/ceramic oscillator (HSEBYP=0)
+    Oscillator,
+    /// external analog clock (low swing) (HSEBYP=1, HSEEXT=0)
+    Bypass,
+    /// external digital clock (full swing) (HSEBYP=1, HSEEXT=1)
+    BypassDigital,
 }
 
-impl Default for ClockSrc {
-    fn default() -> Self {
-        // The default system clock source is MSIS @ 4 MHz, per RM0456 § 11.4.9
-        ClockSrc::MSI(Msirange::RANGE_4MHZ)
-    }
+#[derive(Clone, Copy, Eq, PartialEq)]
+pub struct Hse {
+    /// HSE frequency.
+    pub freq: Hertz,
+    /// HSE mode.
+    pub mode: HseMode,
 }
 
 #[derive(Clone, Copy)]
-pub struct PllConfig {
+pub struct Pll {
     /// The clock source for the PLL.
     pub source: PllSource,
-    /// The PLL prescaler.
+    /// The PLL pre-divider.
     ///
     /// The clock speed of the `source` divided by `m` must be between 4 and 16 MHz.
-    pub m: Pllm,
+    pub prediv: PllPreDiv,
     /// The PLL multiplier.
     ///
     /// The multiplied clock – `source` divided by `m` times `n` – must be between 128 and 544
     /// MHz. The upper limit may be lower depending on the `Config { voltage_range }`.
-    pub n: Plln,
+    pub mul: PllMul,
     /// The divider for the P output.
     ///
     /// The P output is one of several options
     /// that can be used to feed the SAI/MDF/ADF Clock mux's.
-    pub p: Plldiv,
+    pub divp: Option<PllDiv>,
     /// The divider for the Q output.
     ///
     /// The Q ouput is one of severals options that can be used to feed the 48MHz clocks
     /// and the OCTOSPI clock. It may also be used on the MDF/ADF clock mux's.
-    pub q: Plldiv,
+    pub divq: Option<PllDiv>,
     /// The divider for the R output.
     ///
     /// When used to drive the system clock, `source` divided by `m` times `n` divided by `r`
     /// must not exceed 160 MHz. System clocks above 55 MHz require a non-default
     /// `Config { voltage_range }`.
-    pub r: Plldiv,
-}
-
-impl PllConfig {
-    /// A configuration for HSI / 1 * 10 / 1 = 160 MHz
-    pub const fn hsi_160mhz() -> Self {
-        PllConfig {
-            source: PllSource::HSI,
-            m: Pllm::DIV1,
-            n: Plln::MUL10,
-            p: Plldiv::DIV3,
-            q: Plldiv::DIV2,
-            r: Plldiv::DIV1,
-        }
-    }
-
-    /// A configuration for MSIS @ 48 MHz / 3 * 10 / 1 = 160 MHz
-    pub const fn msis_160mhz() -> Self {
-        PllConfig {
-            source: PllSource::MSIS(Msirange::RANGE_48MHZ),
-            m: Pllm::DIV3,
-            n: Plln::MUL10,
-            p: Plldiv::DIV3,
-            q: Plldiv::DIV2,
-            r: Plldiv::DIV1,
-        }
-    }
-}
-
-#[derive(Clone, Copy)]
-pub enum PllSource {
-    /// Use an internal medium speed oscillator as the PLL source.
-    MSIS(Msirange),
-    /// Use the external high speed clock as the system PLL source.
-    ///
-    /// HSE clocks faster than 25 MHz require at least `VoltageScale::RANGE3`, and HSE clocks must
-    /// never exceed 50 MHz.
-    HSE(Hertz),
-    /// Use the 16 MHz internal high speed oscillator as the PLL source.
-    HSI,
+    pub divr: Option<PllDiv>,
 }
 
-impl Into<Pllsrc> for PllSource {
-    fn into(self) -> Pllsrc {
-        match self {
-            PllSource::MSIS(..) => Pllsrc::MSIS,
-            PllSource::HSE(..) => Pllsrc::HSE,
-            PllSource::HSI => Pllsrc::HSI,
-        }
-    }
-}
+pub struct Config {
+    // base clock sources
+    pub msi: Option<MSIRange>,
+    pub hsi: bool,
+    pub hse: Option<Hse>,
+    pub hsi48: Option<super::Hsi48Config>,
 
-impl Into<Sw> for ClockSrc {
-    fn into(self) -> Sw {
-        match self {
-            ClockSrc::MSI(..) => Sw::MSIS,
-            ClockSrc::HSE(..) => Sw::HSE,
-            ClockSrc::HSI => Sw::HSI,
-            ClockSrc::PLL1_R(..) => Sw::PLL1_R,
-        }
-    }
-}
+    // pll
+    pub pll1: Option<Pll>,
+    pub pll2: Option<Pll>,
+    pub pll3: Option<Pll>,
 
-pub struct Config {
+    // sysclk, buses.
     pub mux: ClockSrc,
     pub ahb_pre: AHBPrescaler,
     pub apb1_pre: APBPrescaler,
     pub apb2_pre: APBPrescaler,
     pub apb3_pre: APBPrescaler,
-    pub hsi48: Option<super::Hsi48Config>,
+
     /// The voltage range influences the maximum clock frequencies for different parts of the
     /// device. In particular, system clocks exceeding 110 MHz require `RANGE1`, and system clocks
     /// exceeding 55 MHz require at least `RANGE2`.
@@ -138,35 +87,35 @@ pub struct Config {
     pub ls: super::LsConfig,
 }
 
-impl Config {
-    unsafe fn init_hsi(&self) -> Hertz {
-        RCC.cr().write(|w| w.set_hsion(true));
-        while !RCC.cr().read().hsirdy() {}
-
-        HSI_FREQ
-    }
-
-    unsafe fn init_hse(&self, frequency: Hertz) -> Hertz {
-        // Check frequency limits per RM456 § 11.4.10
-        match self.voltage_range {
-            VoltageScale::RANGE1 | VoltageScale::RANGE2 | VoltageScale::RANGE3 => {
-                assert!(frequency.0 <= 50_000_000);
-            }
-            VoltageScale::RANGE4 => {
-                assert!(frequency.0 <= 25_000_000);
-            }
+impl Default for Config {
+    fn default() -> Self {
+        Self {
+            msi: Some(Msirange::RANGE_4MHZ),
+            hse: None,
+            hsi: false,
+            hsi48: Some(Default::default()),
+            pll1: None,
+            pll2: None,
+            pll3: None,
+            mux: ClockSrc::MSIS,
+            ahb_pre: AHBPrescaler::DIV1,
+            apb1_pre: APBPrescaler::DIV1,
+            apb2_pre: APBPrescaler::DIV1,
+            apb3_pre: APBPrescaler::DIV1,
+            voltage_range: VoltageScale::RANGE1,
+            ls: Default::default(),
         }
-
-        // Enable HSE, and wait for it to stabilize
-        RCC.cr().write(|w| w.set_hseon(true));
-        while !RCC.cr().read().hserdy() {}
-
-        frequency
     }
+}
+
+pub(crate) unsafe fn init(config: Config) {
+    // Set the requested power mode
+    PWR.vosr().modify(|w| w.set_vos(config.voltage_range));
+    while !PWR.vosr().read().vosrdy() {}
 
-    unsafe fn init_msis(&self, range: Msirange) -> Hertz {
+    let msi = config.msi.map(|range| {
         // Check MSI output per RM0456 § 11.4.10
-        match self.voltage_range {
+        match config.voltage_range {
             VoltageScale::RANGE4 => {
                 assert!(msirange_to_hertz(range).0 <= 24_000_000);
             }
@@ -191,223 +140,98 @@ impl Config {
         });
         while !RCC.cr().read().msisrdy() {}
         msirange_to_hertz(range)
-    }
-}
-
-impl Default for Config {
-    fn default() -> Self {
-        Self {
-            mux: ClockSrc::default(),
-            ahb_pre: AHBPrescaler::DIV1,
-            apb1_pre: APBPrescaler::DIV1,
-            apb2_pre: APBPrescaler::DIV1,
-            apb3_pre: APBPrescaler::DIV1,
-            hsi48: Some(Default::default()),
-            voltage_range: VoltageScale::RANGE3,
-            ls: Default::default(),
-        }
-    }
-}
-
-pub(crate) unsafe fn init(config: Config) {
-    // Ensure PWR peripheral clock is enabled
-    RCC.ahb3enr().modify(|w| {
-        w.set_pwren(true);
     });
-    RCC.ahb3enr().read(); // synchronize
 
-    // Set the requested power mode
-    PWR.vosr().modify(|w| {
-        w.set_vos(config.voltage_range);
+    let hsi = config.hsi.then(|| {
+        RCC.cr().write(|w| w.set_hsion(true));
+        while !RCC.cr().read().hsirdy() {}
+
+        HSI_FREQ
     });
-    while !PWR.vosr().read().vosrdy() {}
 
-    let sys_clk = match config.mux {
-        ClockSrc::MSI(range) => config.init_msis(range),
-        ClockSrc::HSE(freq) => config.init_hse(freq),
-        ClockSrc::HSI => config.init_hsi(),
-        ClockSrc::PLL1_R(pll) => {
-            // Configure the PLL source
-            let source_clk = match pll.source {
-                PllSource::MSIS(range) => config.init_msis(range),
-                PllSource::HSE(hertz) => config.init_hse(hertz),
-                PllSource::HSI => config.init_hsi(),
-            };
-
-            // Calculate the reference clock, which is the source divided by m
-            let reference_clk = source_clk / pll.m;
-
-            // Check limits per RM0456 § 11.4.6
-            assert!(Hertz::mhz(4) <= reference_clk && reference_clk <= Hertz::mhz(16));
-
-            // Calculate the PLL1 VCO clock and PLL1 R output clock
-            let pll1_clk = reference_clk * pll.n;
-            let pll1r_clk = pll1_clk / pll.r;
-
-            // Check system clock per RM0456 § 11.4.9
-            assert!(pll1r_clk <= Hertz::mhz(160));
-
-            // Check PLL clocks per RM0456 § 11.4.10
-            match config.voltage_range {
-                VoltageScale::RANGE1 => {
-                    assert!(pll1_clk >= Hertz::mhz(128) && pll1_clk <= Hertz::mhz(544));
-                    assert!(pll1r_clk <= Hertz::mhz(208));
-                }
-                VoltageScale::RANGE2 => {
-                    assert!(pll1_clk >= Hertz::mhz(128) && pll1_clk <= Hertz::mhz(544));
-                    assert!(pll1r_clk <= Hertz::mhz(110));
-                }
-                VoltageScale::RANGE3 => {
-                    assert!(pll1_clk >= Hertz::mhz(128) && pll1_clk <= Hertz::mhz(330));
-                    assert!(pll1r_clk <= Hertz::mhz(55));
-                }
-                VoltageScale::RANGE4 => {
-                    panic!("PLL is unavailable in voltage range 4");
-                }
+    let hse = config.hse.map(|hse| {
+        // Check frequency limits per RM456 § 11.4.10
+        match config.voltage_range {
+            VoltageScale::RANGE1 | VoltageScale::RANGE2 | VoltageScale::RANGE3 => {
+                assert!(hse.freq.0 <= 50_000_000);
             }
+            VoltageScale::RANGE4 => {
+                assert!(hse.freq.0 <= 25_000_000);
+            }
+        }
 
-            // § 10.5.4: if we're targeting >= 55 MHz, we must configure PLL1MBOOST to a prescaler
-            // value that results in an output between 4 and 16 MHz for the PWR EPOD boost
-            let mboost = if pll1r_clk >= Hertz::mhz(55) {
-                // source_clk can be up to 50 MHz, so there's just a few cases:
-                if source_clk > Hertz::mhz(32) {
-                    // Divide by 4, giving EPOD 8-12.5 MHz
-                    Pllmboost::DIV4
-                } else if source_clk > Hertz::mhz(16) {
-                    // Divide by 2, giving EPOD 8-16 MHz
-                    Pllmboost::DIV2
-                } else {
-                    // Bypass, giving EPOD 4-16 MHz
-                    Pllmboost::DIV1
-                }
-            } else {
-                // Nothing to do
-                Pllmboost::DIV1
-            };
-
-            // Disable the PLL, and wait for it to disable
-            RCC.cr().modify(|w| w.set_pllon(0, false));
-            while RCC.cr().read().pllrdy(0) {}
-
-            // Configure the PLL
-            RCC.pll1cfgr().write(|w| {
-                // Configure PLL1 source and prescaler
-                w.set_pllsrc(pll.source.into());
-                w.set_pllm(pll.m);
-
-                // Configure PLL1 input frequncy range
-                let input_range = if reference_clk <= Hertz::mhz(8) {
-                    Pllrge::FREQ_4TO8MHZ
-                } else {
-                    Pllrge::FREQ_8TO16MHZ
-                };
-                w.set_pllrge(input_range);
-
-                // Set the prescaler for PWR EPOD
-                w.set_pllmboost(mboost);
-
-                // Enable PLL1_R output
-                w.set_pllren(true);
+        // Enable HSE, and wait for it to stabilize
+        RCC.cr().write(|w| {
+            w.set_hseon(true);
+            w.set_hsebyp(hse.mode != HseMode::Oscillator);
+            w.set_hseext(match hse.mode {
+                HseMode::Oscillator | HseMode::Bypass => Hseext::ANALOG,
+                HseMode::BypassDigital => Hseext::DIGITAL,
             });
+        });
+        while !RCC.cr().read().hserdy() {}
 
-            // Configure the PLL divisors
-            RCC.pll1divr().modify(|w| {
-                // Set the VCO multiplier
-                w.set_plln(pll.n);
-                w.set_pllp(pll.p);
-                w.set_pllq(pll.q);
-                // Set the R output divisor
-                w.set_pllr(pll.r);
-            });
+        hse.freq
+    });
 
-            // Do we need the EPOD booster to reach the target clock speed per § 10.5.4?
-            if pll1r_clk >= Hertz::mhz(55) {
-                // Enable the booster
-                PWR.vosr().modify(|w| {
-                    w.set_boosten(true);
-                });
-                while !PWR.vosr().read().boostrdy() {}
-            }
+    let hsi48 = config.hsi48.map(super::init_hsi48);
 
-            // Enable the PLL
-            RCC.cr().modify(|w| w.set_pllon(0, true));
-            while !RCC.cr().read().pllrdy(0) {}
+    let pll_input = PllInput { hse, hsi, msi };
+    let pll1 = init_pll(PllInstance::Pll1, config.pll1, &pll_input, config.voltage_range);
+    let pll2 = init_pll(PllInstance::Pll2, config.pll2, &pll_input, config.voltage_range);
+    let pll3 = init_pll(PllInstance::Pll3, config.pll3, &pll_input, config.voltage_range);
 
-            pll1r_clk
-        }
+    let sys_clk = match config.mux {
+        ClockSrc::HSE => hse.unwrap(),
+        ClockSrc::HSI => hsi.unwrap(),
+        ClockSrc::MSIS => msi.unwrap(),
+        ClockSrc::PLL1_R => pll1.r.unwrap(),
     };
 
-    let hsi48 = config.hsi48.map(super::init_hsi48);
+    // Do we need the EPOD booster to reach the target clock speed per § 10.5.4?
+    if sys_clk >= Hertz::mhz(55) {
+        // Enable the booster
+        PWR.vosr().modify(|w| w.set_boosten(true));
+        while !PWR.vosr().read().boostrdy() {}
+    }
 
     // The clock source is ready
     // Calculate and set the flash wait states
     let wait_states = match config.voltage_range {
         // VOS 1 range VCORE 1.26V - 1.40V
-        VoltageScale::RANGE1 => {
-            if sys_clk.0 < 32_000_000 {
-                0
-            } else if sys_clk.0 < 64_000_000 {
-                1
-            } else if sys_clk.0 < 96_000_000 {
-                2
-            } else if sys_clk.0 < 128_000_000 {
-                3
-            } else {
-                4
-            }
-        }
+        VoltageScale::RANGE1 => match sys_clk.0 {
+            ..=32_000_000 => 0,
+            ..=64_000_000 => 1,
+            ..=96_000_000 => 2,
+            ..=128_000_000 => 3,
+            _ => 4,
+        },
         // VOS 2 range VCORE 1.15V - 1.26V
-        VoltageScale::RANGE2 => {
-            if sys_clk.0 < 30_000_000 {
-                0
-            } else if sys_clk.0 < 60_000_000 {
-                1
-            } else if sys_clk.0 < 90_000_000 {
-                2
-            } else {
-                3
-            }
-        }
+        VoltageScale::RANGE2 => match sys_clk.0 {
+            ..=30_000_000 => 0,
+            ..=60_000_000 => 1,
+            ..=90_000_000 => 2,
+            _ => 3,
+        },
         // VOS 3 range VCORE 1.05V - 1.15V
-        VoltageScale::RANGE3 => {
-            if sys_clk.0 < 24_000_000 {
-                0
-            } else if sys_clk.0 < 48_000_000 {
-                1
-            } else {
-                2
-            }
-        }
+        VoltageScale::RANGE3 => match sys_clk.0 {
+            ..=24_000_000 => 0,
+            ..=48_000_000 => 1,
+            _ => 2,
+        },
         // VOS 4 range VCORE 0.95V - 1.05V
-        VoltageScale::RANGE4 => {
-            if sys_clk.0 < 12_000_000 {
-                0
-            } else {
-                1
-            }
-        }
+        VoltageScale::RANGE4 => match sys_clk.0 {
+            ..=12_000_000 => 0,
+            _ => 1,
+        },
     };
     FLASH.acr().modify(|w| {
         w.set_latency(wait_states);
     });
 
     // Switch the system clock source
-    RCC.cfgr1().modify(|w| {
-        w.set_sw(config.mux.into());
-    });
-
-    // RM0456 § 11.4.9 specifies maximum bus frequencies per voltage range, but the maximum bus
-    // frequency for each voltage range exactly matches the maximum permitted PLL output frequency.
-    // Given that:
-    //
-    //   1. Any bus frequency can never exceed the system clock frequency;
-    //   2. We checked the PLL output frequency if we're using it as a system clock;
-    //   3. The maximum HSE frequencies at each voltage range are lower than the bus limits, and
-    //      we checked the HSE frequency if configured as a system clock; and
-    //   4. The maximum frequencies from the other clock sources are lower than the lowest bus
-    //      frequency limit
-    //
-    // ...then we do not need to perform additional bus-related frequency checks.
+    RCC.cfgr1().modify(|w| w.set_sw(config.mux));
+    while RCC.cfgr1().read().sws() != config.mux {}
 
     // Configure the bus prescalers
     RCC.cfgr2().modify(|w| {
@@ -419,64 +243,52 @@ pub(crate) unsafe fn init(config: Config) {
         w.set_ppre3(config.apb3_pre);
     });
 
-    let ahb_freq = sys_clk / config.ahb_pre;
-
-    let (apb1_freq, apb1_tim_freq) = match config.apb1_pre {
-        APBPrescaler::DIV1 => (ahb_freq, ahb_freq),
-        pre => {
-            let freq = ahb_freq / pre;
-            (freq, freq * 2u32)
-        }
-    };
+    let hclk = sys_clk / config.ahb_pre;
 
-    let (apb2_freq, apb2_tim_freq) = match config.apb2_pre {
-        APBPrescaler::DIV1 => (ahb_freq, ahb_freq),
-        pre => {
-            let freq = ahb_freq / pre;
-            (freq, freq * 2u32)
-        }
+    let hclk_max = match config.voltage_range {
+        VoltageScale::RANGE1 => Hertz::mhz(160),
+        VoltageScale::RANGE2 => Hertz::mhz(110),
+        VoltageScale::RANGE3 => Hertz::mhz(55),
+        VoltageScale::RANGE4 => Hertz::mhz(25),
     };
+    assert!(hclk <= hclk_max);
 
-    let (apb3_freq, _apb3_tim_freq) = match config.apb3_pre {
-        APBPrescaler::DIV1 => (ahb_freq, ahb_freq),
-        pre => {
-            let freq = ahb_freq / pre;
-            (freq, freq * 2u32)
-        }
-    };
+    let (pclk1, pclk1_tim) = super::util::calc_pclk(hclk, config.apb1_pre);
+    let (pclk2, pclk2_tim) = super::util::calc_pclk(hclk, config.apb2_pre);
+    let (pclk3, _) = super::util::calc_pclk(hclk, config.apb3_pre);
 
     let rtc = config.ls.init();
 
     set_clocks!(
         sys: Some(sys_clk),
-        hclk1: Some(ahb_freq),
-        hclk2: Some(ahb_freq),
-        hclk3: Some(ahb_freq),
-        pclk1: Some(apb1_freq),
-        pclk2: Some(apb2_freq),
-        pclk3: Some(apb3_freq),
-        pclk1_tim: Some(apb1_tim_freq),
-        pclk2_tim: Some(apb2_tim_freq),
+        hclk1: Some(hclk),
+        hclk2: Some(hclk),
+        hclk3: Some(hclk),
+        pclk1: Some(pclk1),
+        pclk2: Some(pclk2),
+        pclk3: Some(pclk3),
+        pclk1_tim: Some(pclk1_tim),
+        pclk2_tim: Some(pclk2_tim),
         hsi48: hsi48,
         rtc: rtc,
+        hse: hse,
+        hsi: hsi,
+        pll1_p: pll1.p,
+        pll1_q: pll1.q,
+        pll1_r: pll1.r,
+        pll2_p: pll2.p,
+        pll2_q: pll2.q,
+        pll2_r: pll2.r,
+        pll3_p: pll3.p,
+        pll3_q: pll3.q,
+        pll3_r: pll3.r,
 
         // TODO
-        hse: None,
-        hsi: None,
         audioclk: None,
         hsi48_div_2: None,
         lse: None,
         lsi: None,
         msik: None,
-        pll1_p: None,
-        pll1_q: None,
-        pll1_r: None,
-        pll2_p: None,
-        pll2_q: None,
-        pll2_r: None,
-        pll3_p: None,
-        pll3_q: None,
-        pll3_r: None,
         iclk: None,
     );
 }
@@ -501,3 +313,126 @@ fn msirange_to_hertz(range: Msirange) -> Hertz {
         Msirange::RANGE_100KHZ => Hertz(100_000),
     }
 }
+
+pub(super) struct PllInput {
+    pub hsi: Option<Hertz>,
+    pub hse: Option<Hertz>,
+    pub msi: Option<Hertz>,
+}
+
+#[allow(unused)]
+#[derive(Default)]
+pub(super) struct PllOutput {
+    pub p: Option<Hertz>,
+    pub q: Option<Hertz>,
+    pub r: Option<Hertz>,
+}
+
+#[derive(PartialEq, Eq, Clone, Copy)]
+enum PllInstance {
+    Pll1 = 0,
+    Pll2 = 1,
+    Pll3 = 2,
+}
+
+fn pll_enable(instance: PllInstance, enabled: bool) {
+    RCC.cr().modify(|w| w.set_pllon(instance as _, enabled));
+    while RCC.cr().read().pllrdy(instance as _) != enabled {}
+}
+
+fn init_pll(instance: PllInstance, config: Option<Pll>, input: &PllInput, voltage_range: VoltageScale) -> PllOutput {
+    // Disable PLL
+    pll_enable(instance, false);
+
+    let Some(pll) = config else { return PllOutput::default() };
+
+    let src_freq = match pll.source {
+        PllSource::DISABLE => panic!("must not select PLL source as DISABLE"),
+        PllSource::HSE => unwrap!(input.hse),
+        PllSource::HSI => unwrap!(input.hsi),
+        PllSource::MSIS => unwrap!(input.msi),
+    };
+
+    // Calculate the reference clock, which is the source divided by m
+    let ref_freq = src_freq / pll.prediv;
+    // Check limits per RM0456 § 11.4.6
+    assert!(Hertz::mhz(4) <= ref_freq && ref_freq <= Hertz::mhz(16));
+
+    // Check PLL clocks per RM0456 § 11.4.10
+    let (vco_min, vco_max, out_max) = match voltage_range {
+        VoltageScale::RANGE1 => (Hertz::mhz(128), Hertz::mhz(544), Hertz::mhz(208)),
+        VoltageScale::RANGE2 => (Hertz::mhz(128), Hertz::mhz(544), Hertz::mhz(110)),
+        VoltageScale::RANGE3 => (Hertz::mhz(128), Hertz::mhz(330), Hertz::mhz(55)),
+        VoltageScale::RANGE4 => panic!("PLL is unavailable in voltage range 4"),
+    };
+
+    // Calculate the PLL VCO clock
+    let vco_freq = ref_freq * pll.mul;
+    assert!(vco_freq >= vco_min && vco_freq <= vco_max);
+
+    // Calculate output clocks.
+    let p = pll.divp.map(|div| vco_freq / div);
+    let q = pll.divq.map(|div| vco_freq / div);
+    let r = pll.divr.map(|div| vco_freq / div);
+    for freq in [p, q, r] {
+        if let Some(freq) = freq {
+            assert!(freq <= out_max);
+        }
+    }
+
+    let divr = match instance {
+        PllInstance::Pll1 => RCC.pll1divr(),
+        PllInstance::Pll2 => RCC.pll2divr(),
+        PllInstance::Pll3 => RCC.pll3divr(),
+    };
+    divr.write(|w| {
+        w.set_plln(pll.mul);
+        w.set_pllp(pll.divp.unwrap_or(PllDiv::DIV1));
+        w.set_pllq(pll.divq.unwrap_or(PllDiv::DIV1));
+        w.set_pllr(pll.divr.unwrap_or(PllDiv::DIV1));
+    });
+
+    let input_range = match ref_freq.0 {
+        ..=8_000_000 => Pllrge::FREQ_4TO8MHZ,
+        _ => Pllrge::FREQ_8TO16MHZ,
+    };
+
+    macro_rules! write_fields {
+        ($w:ident) => {
+            $w.set_pllpen(pll.divp.is_some());
+            $w.set_pllqen(pll.divq.is_some());
+            $w.set_pllren(pll.divr.is_some());
+            $w.set_pllm(pll.prediv);
+            $w.set_pllsrc(pll.source);
+            $w.set_pllrge(input_range);
+        };
+    }
+
+    match instance {
+        PllInstance::Pll1 => RCC.pll1cfgr().write(|w| {
+            // § 10.5.4: if we're targeting >= 55 MHz, we must configure PLL1MBOOST to a prescaler
+            // value that results in an output between 4 and 16 MHz for the PWR EPOD boost
+            if r.unwrap() >= Hertz::mhz(55) {
+                // source_clk can be up to 50 MHz, so there's just a few cases:
+                let mboost = match src_freq.0 {
+                    ..=16_000_000 => Pllmboost::DIV1, // Bypass, giving EPOD 4-16 MHz
+                    ..=32_000_000 => Pllmboost::DIV2, // Divide by 2, giving EPOD 8-16 MHz
+                    _ => Pllmboost::DIV4,             // Divide by 4, giving EPOD 8-12.5 MHz
+                };
+                w.set_pllmboost(mboost);
+            }
+            write_fields!(w);
+        }),
+        PllInstance::Pll2 => RCC.pll2cfgr().write(|w| {
+            write_fields!(w);
+        }),
+        PllInstance::Pll3 => RCC.pll3cfgr().write(|w| {
+            write_fields!(w);
+        }),
+    }
+
+    // Enable PLL
+    pll_enable(instance, true);
+
+    PllOutput { p, q, r }
+}
diff --git a/embassy-stm32/src/timer/mod.rs b/embassy-stm32/src/timer/mod.rs
index 9480d6972..9397da2a1 100644
--- a/embassy-stm32/src/timer/mod.rs
+++ b/embassy-stm32/src/timer/mod.rs
@@ -65,17 +65,17 @@ pub(crate) mod sealed {
         fn regs_core() -> crate::pac::timer::TimCore;
 
         /// Start the timer.
-        fn start(&mut self) {
+        fn start(&self) {
             Self::regs_core().cr1().modify(|r| r.set_cen(true));
         }
 
         /// Stop the timer.
-        fn stop(&mut self) {
+        fn stop(&self) {
             Self::regs_core().cr1().modify(|r| r.set_cen(false));
         }
 
         /// Reset the counter value to 0
-        fn reset(&mut self) {
+        fn reset(&self) {
             Self::regs_core().cnt().write(|r| r.set_cnt(0));
         }
 
@@ -85,7 +85,7 @@ pub(crate) mod sealed {
         /// the timer counter will wrap around at the same frequency as is being set.
         /// In center-aligned mode (which not all timers support), the wrap-around frequency is effectively halved
         /// because it needs to count up and down.
-        fn set_frequency(&mut self, frequency: Hertz) {
+        fn set_frequency(&self, frequency: Hertz) {
             let f = frequency.0;
             let timer_f = Self::frequency().0;
             assert!(f > 0);
@@ -108,7 +108,7 @@ pub(crate) mod sealed {
         /// Clear update interrupt.
         ///
         /// Returns whether the update interrupt flag was set.
-        fn clear_update_interrupt(&mut self) -> bool {
+        fn clear_update_interrupt(&self) -> bool {
             let regs = Self::regs_core();
             let sr = regs.sr().read();
             if sr.uif() {
@@ -122,12 +122,12 @@ pub(crate) mod sealed {
         }
 
         /// Enable/disable the update interrupt.
-        fn enable_update_interrupt(&mut self, enable: bool) {
+        fn enable_update_interrupt(&self, enable: bool) {
             Self::regs_core().dier().modify(|r| r.set_uie(enable));
         }
 
         /// Enable/disable autoreload preload.
-        fn set_autoreload_preload(&mut self, enable: bool) {
+        fn set_autoreload_preload(&self, enable: bool) {
             Self::regs_core().cr1().modify(|r| r.set_arpe(enable));
         }
 
@@ -154,7 +154,7 @@ pub(crate) mod sealed {
         fn regs_basic_no_cr2() -> crate::pac::timer::TimBasicNoCr2;
 
         /// Enable/disable the update dma.
-        fn enable_update_dma(&mut self, enable: bool) {
+        fn enable_update_dma(&self, enable: bool) {
             Self::regs_basic_no_cr2().dier().modify(|r| r.set_ude(enable));
         }
 
@@ -186,7 +186,7 @@ pub(crate) mod sealed {
         fn regs_1ch() -> crate::pac::timer::Tim1ch;
 
         /// Set clock divider.
-        fn set_clock_division(&mut self, ckd: vals::Ckd) {
+        fn set_clock_division(&self, ckd: vals::Ckd) {
             Self::regs_1ch().cr1().modify(|r| r.set_ckd(ckd));
         }
 
@@ -218,7 +218,7 @@ pub(crate) mod sealed {
         fn regs_gp16() -> crate::pac::timer::TimGp16;
 
         /// Set counting mode.
-        fn set_counting_mode(&mut self, mode: CountingMode) {
+        fn set_counting_mode(&self, mode: CountingMode) {
             let (cms, dir) = mode.into();
 
             let timer_enabled = Self::regs_core().cr1().read().cen();
@@ -237,7 +237,7 @@ pub(crate) mod sealed {
         }
 
         /// Set input capture filter.
-        fn set_input_capture_filter(&mut self, channel: Channel, icf: vals::FilterValue) {
+        fn set_input_capture_filter(&self, channel: Channel, icf: vals::FilterValue) {
             let raw_channel = channel.index();
             Self::regs_gp16()
                 .ccmr_input(raw_channel / 2)
@@ -245,17 +245,17 @@ pub(crate) mod sealed {
         }
 
         /// Clear input interrupt.
-        fn clear_input_interrupt(&mut self, channel: Channel) {
+        fn clear_input_interrupt(&self, channel: Channel) {
             Self::regs_gp16().sr().modify(|r| r.set_ccif(channel.index(), false));
         }
 
         /// Enable input interrupt.
-        fn enable_input_interrupt(&mut self, channel: Channel, enable: bool) {
+        fn enable_input_interrupt(&self, channel: Channel, enable: bool) {
             Self::regs_gp16().dier().modify(|r| r.set_ccie(channel.index(), enable));
         }
 
         /// Set input capture prescaler.
-        fn set_input_capture_prescaler(&mut self, channel: Channel, factor: u8) {
+        fn set_input_capture_prescaler(&self, channel: Channel, factor: u8) {
             let raw_channel = channel.index();
             Self::regs_gp16()
                 .ccmr_input(raw_channel / 2)
@@ -263,7 +263,7 @@ pub(crate) mod sealed {
         }
 
         /// Set input TI selection.
-        fn set_input_ti_selection(&mut self, channel: Channel, tisel: InputTISelection) {
+        fn set_input_ti_selection(&self, channel: Channel, tisel: InputTISelection) {
             let raw_channel = channel.index();
             Self::regs_gp16()
                 .ccmr_input(raw_channel / 2)
@@ -271,7 +271,7 @@ pub(crate) mod sealed {
         }
 
         /// Set input capture mode.
-        fn set_input_capture_mode(&mut self, channel: Channel, mode: InputCaptureMode) {
+        fn set_input_capture_mode(&self, channel: Channel, mode: InputCaptureMode) {
             Self::regs_gp16().ccer().modify(|r| match mode {
                 InputCaptureMode::Rising => {
                     r.set_ccnp(channel.index(), false);
@@ -289,7 +289,7 @@ pub(crate) mod sealed {
         }
 
         /// Set output compare mode.
-        fn set_output_compare_mode(&mut self, channel: Channel, mode: OutputCompareMode) {
+        fn set_output_compare_mode(&self, channel: Channel, mode: OutputCompareMode) {
             let raw_channel: usize = channel.index();
             Self::regs_gp16()
                 .ccmr_output(raw_channel / 2)
@@ -297,14 +297,14 @@ pub(crate) mod sealed {
         }
 
         /// Set output polarity.
-        fn set_output_polarity(&mut self, channel: Channel, polarity: OutputPolarity) {
+        fn set_output_polarity(&self, channel: Channel, polarity: OutputPolarity) {
             Self::regs_gp16()
                 .ccer()
                 .modify(|w| w.set_ccp(channel.index(), polarity.into()));
         }
 
         /// Enable/disable a channel.
-        fn enable_channel(&mut self, channel: Channel, enable: bool) {
+        fn enable_channel(&self, channel: Channel, enable: bool) {
             Self::regs_gp16().ccer().modify(|w| w.set_cce(channel.index(), enable));
         }
 
@@ -314,12 +314,12 @@ pub(crate) mod sealed {
         }
 
         /// Set compare value for a channel.
-        fn set_compare_value(&mut self, channel: Channel, value: u16) {
+        fn set_compare_value(&self, channel: Channel, value: u16) {
             Self::regs_gp16().ccr(channel.index()).modify(|w| w.set_ccr(value));
         }
 
         /// Get capture value for a channel.
-        fn get_capture_value(&mut self, channel: Channel) -> u16 {
+        fn get_capture_value(&self, channel: Channel) -> u16 {
             Self::regs_gp16().ccr(channel.index()).read().ccr()
         }
 
@@ -329,7 +329,7 @@ pub(crate) mod sealed {
         }
 
         /// Set output compare preload.
-        fn set_output_compare_preload(&mut self, channel: Channel, preload: bool) {
+        fn set_output_compare_preload(&self, channel: Channel, preload: bool) {
             let channel_index = channel.index();
             Self::regs_gp16()
                 .ccmr_output(channel_index / 2)
@@ -342,7 +342,7 @@ pub(crate) mod sealed {
         }
 
         /// Set capture compare DMA selection
-        fn set_cc_dma_selection(&mut self, ccds: super::vals::Ccds) {
+        fn set_cc_dma_selection(&self, ccds: super::vals::Ccds) {
             Self::regs_gp16().cr2().modify(|w| w.set_ccds(ccds))
         }
 
@@ -352,7 +352,7 @@ pub(crate) mod sealed {
         }
 
         /// Set capture compare DMA enable state
-        fn set_cc_dma_enable_state(&mut self, channel: Channel, ccde: bool) {
+        fn set_cc_dma_enable_state(&self, channel: Channel, ccde: bool) {
             Self::regs_gp16().dier().modify(|w| w.set_ccde(channel.index(), ccde))
         }
     }
@@ -369,7 +369,7 @@ pub(crate) mod sealed {
         fn regs_gp32() -> crate::pac::timer::TimGp32;
 
         /// Set timer frequency.
-        fn set_frequency(&mut self, frequency: Hertz) {
+        fn set_frequency(&self, frequency: Hertz) {
             let f = frequency.0;
             assert!(f > 0);
             let timer_f = Self::frequency().0;
@@ -398,12 +398,12 @@ pub(crate) mod sealed {
         }
 
         /// Set comapre value for a channel.
-        fn set_compare_value(&mut self, channel: Channel, value: u32) {
+        fn set_compare_value(&self, channel: Channel, value: u32) {
             Self::regs_gp32().ccr(channel.index()).modify(|w| w.set_ccr(value));
         }
 
         /// Get capture value for a channel.
-        fn get_capture_value(&mut self, channel: Channel) -> u32 {
+        fn get_capture_value(&self, channel: Channel) -> u32 {
             Self::regs_gp32().ccr(channel.index()).read().ccr()
         }
 
@@ -430,17 +430,17 @@ pub(crate) mod sealed {
         fn regs_1ch_cmp() -> crate::pac::timer::Tim1chCmp;
 
         /// Set clock divider for the dead time.
-        fn set_dead_time_clock_division(&mut self, value: vals::Ckd) {
+        fn set_dead_time_clock_division(&self, value: vals::Ckd) {
             Self::regs_1ch_cmp().cr1().modify(|w| w.set_ckd(value));
         }
 
         /// Set dead time, as a fraction of the max duty value.
-        fn set_dead_time_value(&mut self, value: u8) {
+        fn set_dead_time_value(&self, value: u8) {
             Self::regs_1ch_cmp().bdtr().modify(|w| w.set_dtg(value));
         }
 
         /// Enable timer outputs.
-        fn enable_outputs(&mut self) {
+        fn enable_outputs(&self) {
             Self::regs_1ch_cmp().bdtr().modify(|w| w.set_moe(true));
         }
     }
@@ -468,14 +468,14 @@ pub(crate) mod sealed {
         fn regs_advanced() -> crate::pac::timer::TimAdv;
 
         /// Set complementary output polarity.
-        fn set_complementary_output_polarity(&mut self, channel: Channel, polarity: OutputPolarity) {
+        fn set_complementary_output_polarity(&self, channel: Channel, polarity: OutputPolarity) {
             Self::regs_advanced()
                 .ccer()
                 .modify(|w| w.set_ccnp(channel.index(), polarity.into()));
         }
 
         /// Enable/disable a complementary channel.
-        fn enable_complementary_channel(&mut self, channel: Channel, enable: bool) {
+        fn enable_complementary_channel(&self, channel: Channel, enable: bool) {
             Self::regs_advanced()
                 .ccer()
                 .modify(|w| w.set_ccne(channel.index(), enable));
diff --git a/embassy-stm32/src/usb/usb.rs b/embassy-stm32/src/usb/usb.rs
index 34d6b52fd..be321a19b 100644
--- a/embassy-stm32/src/usb/usb.rs
+++ b/embassy-stm32/src/usb/usb.rs
@@ -264,7 +264,7 @@ impl<'d, T: Instance> Driver<'d, T> {
 
         let regs = T::regs();
 
-        #[cfg(any(stm32l5, stm32wb))]
+        #[cfg(any(stm32l4, stm32l5, stm32wb))]
         crate::pac::PWR.cr2().modify(|w| w.set_usv(true));
 
         #[cfg(pwr_h5)]