Merge pull request #2693 from cschuhen/feature/bxcan_use_frame

Use the same Frame struct for BXCAN and FDCAN.

11 files changed, 186 insertions, 502 deletions

diff --git a/embassy-stm32/src/can/bx/frame.rs b/embassy-stm32/src/can/bx/frame.rs
deleted file mode 100644
index 828f375be..000000000
--- a/embassy-stm32/src/can/bx/frame.rs
+++ /dev/null
@@ -1,248 +0,0 @@
-#[cfg(test)]
-use core::cmp::Ordering;
-use core::ops::{Deref, DerefMut};
-
-use crate::can::bx::{Id, IdReg};
-
-/// A CAN data or remote frame.
-#[derive(Clone, Debug, Eq)]
-//#[cfg_attr(feature = "defmt", derive(defmt::Format))]
-#[cfg_attr(feature = "defmt", derive(defmt::Format))]
-pub struct Frame {
-    pub(crate) id: IdReg,
-    pub(crate) data: Data,
-}
-
-impl Frame {
-    /// Creates a new data frame.
-    pub fn new_data(id: impl Into<Id>, data: impl Into<Data>) -> Self {
-        let id = match id.into() {
-            Id::Standard(id) => IdReg::new_standard(id),
-            Id::Extended(id) => IdReg::new_extended(id),
-        };
-
-        Self { id, data: data.into() }
-    }
-
-    /// Creates a new remote frame with configurable data length code (DLC).
-    ///
-    /// # Panics
-    ///
-    /// This function will panic if `dlc` is not inside the valid range `0..=8`.
-    pub fn new_remote(id: impl Into<Id>, dlc: u8) -> Self {
-        assert!(dlc <= 8);
-
-        let mut frame = Self::new_data(id, []);
-        // Just extend the data length, even with no data present. The API does not hand out this
-        // `Data` object.
-        frame.data.len = dlc;
-        frame.id = frame.id.with_rtr(true);
-        frame
-    }
-
-    /// Returns true if this frame is an extended frame.
-    #[inline]
-    pub fn is_extended(&self) -> bool {
-        self.id.is_extended()
-    }
-
-    /// Returns true if this frame is a standard frame.
-    #[inline]
-    pub fn is_standard(&self) -> bool {
-        self.id.is_standard()
-    }
-
-    /// Returns true if this frame is a remote frame.
-    #[inline]
-    pub fn is_remote_frame(&self) -> bool {
-        self.id.rtr()
-    }
-
-    /// Returns true if this frame is a data frame.
-    #[inline]
-    pub fn is_data_frame(&self) -> bool {
-        !self.is_remote_frame()
-    }
-
-    /// Returns the frame identifier.
-    #[inline]
-    pub fn id(&self) -> Id {
-        self.id.to_id()
-    }
-
-    /// Returns the priority of this frame.
-    #[inline]
-    pub fn priority(&self) -> FramePriority {
-        FramePriority(self.id)
-    }
-
-    /// Returns the data length code (DLC) which is in the range 0..8.
-    ///
-    /// For data frames the DLC value always matches the length of the data.
-    /// Remote frames do not carry any data, yet the DLC can be greater than 0.
-    #[inline]
-    pub fn dlc(&self) -> u8 {
-        self.data.len() as u8
-    }
-
-    /// Returns the frame data (0..8 bytes in length) if this is a data frame.
-    ///
-    /// If this is a remote frame, returns `None`.
-    pub fn data(&self) -> Option<&Data> {
-        if self.is_data_frame() {
-            Some(&self.data)
-        } else {
-            None
-        }
-    }
-}
-
-impl PartialEq for Frame {
-    fn eq(&self, other: &Self) -> bool {
-        match (self.data(), other.data()) {
-            (None, None) => self.id.eq(&other.id),
-            (Some(a), Some(b)) => self.id.eq(&other.id) && a.eq(b),
-            (None, Some(_)) | (Some(_), None) => false,
-        }
-    }
-}
-
-/// Priority of a CAN frame.
-///
-/// Returned by [`Frame::priority`].
-///
-/// The priority of a frame is determined by the bits that are part of the *arbitration field*.
-/// These consist of the frame identifier bits (including the *IDE* bit, which is 0 for extended
-/// frames and 1 for standard frames), as well as the *RTR* bit, which determines whether a frame
-/// is a data or remote frame. Lower values of the *arbitration field* have higher priority.
-///
-/// This struct wraps the *arbitration field* and implements `PartialOrd` and `Ord` accordingly,
-/// ordering higher priorities greater than lower ones.
-#[derive(Debug, Copy, Clone)]
-pub struct FramePriority(IdReg);
-
-/// Ordering is based on the Identifier and frame type (data vs. remote) and can be used to sort
-/// frames by priority.
-impl Ord for FramePriority {
-    fn cmp(&self, other: &Self) -> core::cmp::Ordering {
-        self.0.cmp(&other.0)
-    }
-}
-
-impl PartialOrd for FramePriority {
-    fn partial_cmp(&self, other: &Self) -> Option<core::cmp::Ordering> {
-        Some(self.cmp(other))
-    }
-}
-
-impl PartialEq for FramePriority {
-    fn eq(&self, other: &Self) -> bool {
-        self.cmp(other) == core::cmp::Ordering::Equal
-    }
-}
-
-impl Eq for FramePriority {}
-
-/// Payload of a CAN data frame.
-///
-/// Contains 0 to 8 Bytes of data.
-///
-/// `Data` implements `From<[u8; N]>` for all `N` up to 8, which provides a convenient lossless
-/// conversion from fixed-length arrays.
-#[derive(Debug, Copy, Clone)]
-pub struct Data {
-    pub(crate) len: u8,
-    pub(crate) bytes: [u8; 8],
-}
-
-impl Data {
-    /// Creates a data payload from a raw byte slice.
-    ///
-    /// Returns `None` if `data` contains more than 8 Bytes (which is the maximum).
-    ///
-    /// `Data` can also be constructed from fixed-length arrays up to length 8 via `From`/`Into`.
-    pub fn new(data: &[u8]) -> Option<Self> {
-        if data.len() > 8 {
-            return None;
-        }
-
-        let mut bytes = [0; 8];
-        bytes[..data.len()].copy_from_slice(data);
-
-        Some(Self {
-            len: data.len() as u8,
-            bytes,
-        })
-    }
-
-    /// Creates an empty data payload containing 0 bytes.
-    #[inline]
-    pub const fn empty() -> Self {
-        Self { len: 0, bytes: [0; 8] }
-    }
-}
-
-impl Deref for Data {
-    type Target = [u8];
-
-    #[inline]
-    fn deref(&self) -> &[u8] {
-        &self.bytes[..usize::from(self.len)]
-    }
-}
-
-impl DerefMut for Data {
-    #[inline]
-    fn deref_mut(&mut self) -> &mut [u8] {
-        &mut self.bytes[..usize::from(self.len)]
-    }
-}
-
-impl AsRef<[u8]> for Data {
-    #[inline]
-    fn as_ref(&self) -> &[u8] {
-        self.deref()
-    }
-}
-
-impl AsMut<[u8]> for Data {
-    #[inline]
-    fn as_mut(&mut self) -> &mut [u8] {
-        self.deref_mut()
-    }
-}
-
-impl PartialEq for Data {
-    fn eq(&self, other: &Self) -> bool {
-        self.as_ref() == other.as_ref()
-    }
-}
-
-impl Eq for Data {}
-
-#[cfg(feature = "defmt")]
-impl defmt::Format for Data {
-    fn format(&self, fmt: defmt::Formatter<'_>) {
-        self.as_ref().format(fmt)
-    }
-}
-
-macro_rules! data_from_array {
-    ( $($len:literal),+ ) => {
-        $(
-            impl From<[u8; $len]> for Data {
-                #[inline]
-                fn from(arr: [u8; $len]) -> Self {
-                    let mut bytes = [0; 8];
-                    bytes[..$len].copy_from_slice(&arr);
-                    Self {
-                        len: $len,
-                        bytes,
-                    }
-                }
-            }
-        )+
-    };
-}
-
-data_from_array!(0, 1, 2, 3, 4, 5, 6, 7, 8);
diff --git a/embassy-stm32/src/can/bx/id.rs b/embassy-stm32/src/can/bx/id.rs
deleted file mode 100644
index 9fdcd8319..000000000
--- a/embassy-stm32/src/can/bx/id.rs
+++ /dev/null
@@ -1,113 +0,0 @@
-//! CAN Identifiers.
-
-/// Standard 11-bit CAN Identifier (`0..=0x7FF`).
-#[derive(Debug, Copy, Clone, Eq, PartialEq)]
-pub struct StandardId(u16);
-
-impl StandardId {
-    /// CAN ID `0`, the highest priority.
-    pub const ZERO: Self = Self(0);
-
-    /// CAN ID `0x7FF`, the lowest priority.
-    pub const MAX: Self = Self(0x7FF);
-
-    /// Tries to create a `StandardId` from a raw 16-bit integer.
-    ///
-    /// This will return `None` if `raw` is out of range of an 11-bit integer (`> 0x7FF`).
-    #[inline]
-    pub const fn new(raw: u16) -> Option<Self> {
-        if raw <= 0x7FF {
-            Some(Self(raw))
-        } else {
-            None
-        }
-    }
-
-    /// Creates a new `StandardId` without checking if it is inside the valid range.
-    ///
-    /// # Safety
-    ///
-    /// The caller must ensure that `raw` is in the valid range, otherwise the behavior is
-    /// undefined.
-    #[inline]
-    pub const unsafe fn new_unchecked(raw: u16) -> Self {
-        Self(raw)
-    }
-
-    /// Returns this CAN Identifier as a raw 16-bit integer.
-    #[inline]
-    pub fn as_raw(&self) -> u16 {
-        self.0
-    }
-}
-
-/// Extended 29-bit CAN Identifier (`0..=1FFF_FFFF`).
-#[derive(Debug, Copy, Clone, Eq, PartialEq)]
-pub struct ExtendedId(u32);
-
-impl ExtendedId {
-    /// CAN ID `0`, the highest priority.
-    pub const ZERO: Self = Self(0);
-
-    /// CAN ID `0x1FFFFFFF`, the lowest priority.
-    pub const MAX: Self = Self(0x1FFF_FFFF);
-
-    /// Tries to create a `ExtendedId` from a raw 32-bit integer.
-    ///
-    /// This will return `None` if `raw` is out of range of an 29-bit integer (`> 0x1FFF_FFFF`).
-    #[inline]
-    pub const fn new(raw: u32) -> Option<Self> {
-        if raw <= 0x1FFF_FFFF {
-            Some(Self(raw))
-        } else {
-            None
-        }
-    }
-
-    /// Creates a new `ExtendedId` without checking if it is inside the valid range.
-    ///
-    /// # Safety
-    ///
-    /// The caller must ensure that `raw` is in the valid range, otherwise the behavior is
-    /// undefined.
-    #[inline]
-    pub const unsafe fn new_unchecked(raw: u32) -> Self {
-        Self(raw)
-    }
-
-    /// Returns this CAN Identifier as a raw 32-bit integer.
-    #[inline]
-    pub fn as_raw(&self) -> u32 {
-        self.0
-    }
-
-    /// Returns the Base ID part of this extended identifier.
-    pub fn standard_id(&self) -> StandardId {
-        // ID-28 to ID-18
-        StandardId((self.0 >> 18) as u16)
-    }
-}
-
-/// A CAN Identifier (standard or extended).
-#[derive(Debug, Copy, Clone, Eq, PartialEq)]
-pub enum Id {
-    /// Standard 11-bit Identifier (`0..=0x7FF`).
-    Standard(StandardId),
-
-    /// Extended 29-bit Identifier (`0..=0x1FFF_FFFF`).
-    Extended(ExtendedId),
-}
-
-impl From<StandardId> for Id {
-    #[inline]
-    fn from(id: StandardId) -> Self {
-        Id::Standard(id)
-    }
-}
-
-impl From<ExtendedId> for Id {
-    #[inline]
-    fn from(id: ExtendedId) -> Self {
-        Id::Extended(id)
-    }
-}
diff --git a/embassy-stm32/src/can/bx/mod.rs b/embassy-stm32/src/can/bx/mod.rs
index f639260a1..33e702c6e 100644
--- a/embassy-stm32/src/can/bx/mod.rs
+++ b/embassy-stm32/src/can/bx/mod.rs
@@ -25,19 +25,25 @@
 
 //mod embedded_hal;
 pub mod filter;
-mod frame;
-mod id;
 
 #[allow(clippy::all)] // generated code
 use core::cmp::{Ord, Ordering};
-use core::convert::{Infallible, TryInto};
+use core::convert::{Infallible, Into, TryInto};
 use core::marker::PhantomData;
 use core::mem;
 
-pub use id::{ExtendedId, Id, StandardId};
+pub use embedded_can::{ExtendedId, Id, StandardId};
+
+/// CAN Header: includes ID and length
+pub type Header = crate::can::frame::Header;
+
+/// Data for a CAN Frame
+pub type Data = crate::can::frame::ClassicData;
+
+/// CAN Frame
+pub type Frame = crate::can::frame::ClassicFrame;
 
 use crate::can::bx::filter::MasterFilters;
-pub use crate::can::bx::frame::{Data, Frame, FramePriority};
 
 /// A bxCAN peripheral instance.
 ///
@@ -145,23 +151,25 @@ impl IdReg {
         Self(reg & 0xFFFF_FFFE)
     }
 
-    /// Sets the remote transmission (RTR) flag. This marks the identifier as
-    /// being part of a remote frame.
-    #[must_use = "returns a new IdReg without modifying `self`"]
-    fn with_rtr(self, rtr: bool) -> IdReg {
-        if rtr {
-            Self(self.0 | Self::RTR_MASK)
+    /// Returns the identifier.
+    fn to_id(self) -> Id {
+        if self.is_extended() {
+            Id::Extended(unsafe { ExtendedId::new_unchecked(self.0 >> Self::EXTENDED_SHIFT) })
         } else {
-            Self(self.0 & !Self::RTR_MASK)
+            Id::Standard(unsafe { StandardId::new_unchecked((self.0 >> Self::STANDARD_SHIFT) as u16) })
         }
     }
 
     /// Returns the identifier.
-    fn to_id(self) -> Id {
+    fn id(self) -> embedded_can::Id {
         if self.is_extended() {
-            Id::Extended(unsafe { ExtendedId::new_unchecked(self.0 >> Self::EXTENDED_SHIFT) })
+            embedded_can::ExtendedId::new(self.0 >> Self::EXTENDED_SHIFT)
+                .unwrap()
+                .into()
         } else {
-            Id::Standard(unsafe { StandardId::new_unchecked((self.0 >> Self::STANDARD_SHIFT) as u16) })
+            embedded_can::StandardId::new((self.0 >> Self::STANDARD_SHIFT) as u16)
+                .unwrap()
+                .into()
         }
     }
 
@@ -170,17 +178,27 @@ impl IdReg {
         self.0 & Self::IDE_MASK != 0
     }
 
-    /// Returns `true` if the identifier is a standard identifier.
-    fn is_standard(self) -> bool {
-        !self.is_extended()
-    }
-
     /// Returns `true` if the identifer is part of a remote frame (RTR bit set).
     fn rtr(self) -> bool {
         self.0 & Self::RTR_MASK != 0
     }
 }
 
+impl From<&embedded_can::Id> for IdReg {
+    fn from(eid: &embedded_can::Id) -> Self {
+        match eid {
+            embedded_can::Id::Standard(id) => IdReg::new_standard(StandardId::new(id.as_raw()).unwrap()),
+            embedded_can::Id::Extended(id) => IdReg::new_extended(ExtendedId::new(id.as_raw()).unwrap()),
+        }
+    }
+}
+
+impl From<IdReg> for embedded_can::Id {
+    fn from(idr: IdReg) -> Self {
+        idr.id()
+    }
+}
+
 /// `IdReg` is ordered by priority.
 impl Ord for IdReg {
     fn cmp(&self, other: &Self) -> Ordering {
@@ -682,9 +700,9 @@ where
             // The controller schedules pending frames of same priority based on the
             // mailbox index instead. As a workaround check all pending mailboxes
             // and only accept higher priority frames.
-            self.check_priority(0, frame.id)?;
-            self.check_priority(1, frame.id)?;
-            self.check_priority(2, frame.id)?;
+            self.check_priority(0, frame.id().into())?;
+            self.check_priority(1, frame.id().into())?;
+            self.check_priority(2, frame.id().into())?;
 
             let all_frames_are_pending = !tsr.tme(0) && !tsr.tme(1) && !tsr.tme(2);
             if all_frames_are_pending {
@@ -739,14 +757,15 @@ where
         debug_assert!(idx < 3);
 
         let mb = self.canregs.tx(idx);
-        mb.tdtr().write(|w| w.set_dlc(frame.dlc() as u8));
+        mb.tdtr().write(|w| w.set_dlc(frame.header().len() as u8));
 
         mb.tdlr()
-            .write(|w| w.0 = u32::from_ne_bytes(frame.data.bytes[0..4].try_into().unwrap()));
+            .write(|w| w.0 = u32::from_ne_bytes(frame.data()[0..4].try_into().unwrap()));
         mb.tdhr()
-            .write(|w| w.0 = u32::from_ne_bytes(frame.data.bytes[4..8].try_into().unwrap()));
+            .write(|w| w.0 = u32::from_ne_bytes(frame.data()[4..8].try_into().unwrap()));
+        let id: IdReg = frame.id().into();
         mb.tir().write(|w| {
-            w.0 = frame.id.0;
+            w.0 = id.0;
             w.set_txrq(true);
         });
     }
@@ -756,16 +775,14 @@ where
             debug_assert!(idx < 3);
 
             let mb = self.canregs.tx(idx);
-            // Read back the pending frame.
-            let mut pending_frame = Frame {
-                id: IdReg(mb.tir().read().0),
-                data: Data::empty(),
-            };
-            pending_frame.data.bytes[0..4].copy_from_slice(&mb.tdlr().read().0.to_ne_bytes());
-            pending_frame.data.bytes[4..8].copy_from_slice(&mb.tdhr().read().0.to_ne_bytes());
-            pending_frame.data.len = mb.tdtr().read().dlc();
-
-            Some(pending_frame)
+
+            let id = IdReg(mb.tir().read().0).id();
+            let mut data = [0xff; 8];
+            data[0..4].copy_from_slice(&mb.tdlr().read().0.to_ne_bytes());
+            data[4..8].copy_from_slice(&mb.tdhr().read().0.to_ne_bytes());
+            let len = mb.tdtr().read().dlc();
+
+            Some(Frame::new(Header::new(id, len, false), &data).unwrap())
         } else {
             // Abort request failed because the frame was already sent (or being sent) on
             // the bus. All mailboxes are now free. This can happen for small prescaler
@@ -898,18 +915,16 @@ fn receive_fifo(canregs: crate::pac::can::Can, fifo_nr: usize) -> nb::Result<Fra
     }
 
     // Read the frame.
-    let mut frame = Frame {
-        id: IdReg(rx.rir().read().0),
-        data: [0; 8].into(),
-    };
-    frame.data[0..4].copy_from_slice(&rx.rdlr().read().0.to_ne_bytes());
-    frame.data[4..8].copy_from_slice(&rx.rdhr().read().0.to_ne_bytes());
-    frame.data.len = rx.rdtr().read().dlc();
+    let id = IdReg(rx.rir().read().0).id();
+    let mut data = [0xff; 8];
+    data[0..4].copy_from_slice(&rx.rdlr().read().0.to_ne_bytes());
+    data[4..8].copy_from_slice(&rx.rdhr().read().0.to_ne_bytes());
+    let len = rx.rdtr().read().dlc();
 
     // Release the mailbox.
     rfr.write(|w| w.set_rfom(true));
 
-    Ok(frame)
+    Ok(Frame::new(Header::new(id, len, false), &data).unwrap())
 }
 
 /// Identifies one of the two receive FIFOs.
diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs
index acd831937..bb7cc3d7f 100644
--- a/embassy-stm32/src/can/bxcan.rs
+++ b/embassy-stm32/src/can/bxcan.rs
@@ -6,7 +6,7 @@ use core::task::Poll;
 
 pub mod bx;
 
-pub use bx::{filter, Data, ExtendedId, Fifo, Frame, Id, StandardId};
+pub use bx::{filter, Data, ExtendedId, Fifo, Frame, Header, Id, StandardId};
 use embassy_hal_internal::{into_ref, PeripheralRef};
 use futures::FutureExt;
 
@@ -18,19 +18,20 @@ use crate::{interrupt, peripherals, Peripheral};
 
 pub mod enums;
 use enums::*;
+pub mod frame;
 pub mod util;
 
 /// Contains CAN frame and additional metadata.
 ///
 /// Timestamp is available if `time` feature is enabled.
-#[derive(Debug, Clone, PartialEq, Eq)]
+#[derive(Debug, Clone)]
 #[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct Envelope {
     /// Reception time.
     #[cfg(feature = "time")]
     pub ts: embassy_time::Instant,
     /// The actual CAN frame.
-    pub frame: crate::can::bx::Frame,
+    pub frame: Frame,
 }
 
 /// Interrupt handler.
@@ -260,20 +261,20 @@ impl<'d, T: Instance> Can<'d, T> {
             }
 
             let rir = fifo.rir().read();
-            let id = if rir.ide() == Ide::STANDARD {
-                Id::from(StandardId::new_unchecked(rir.stid()))
+            let id: embedded_can::Id = if rir.ide() == Ide::STANDARD {
+                embedded_can::StandardId::new(rir.stid()).unwrap().into()
             } else {
                 let stid = (rir.stid() & 0x7FF) as u32;
                 let exid = rir.exid() & 0x3FFFF;
                 let id = (stid << 18) | (exid);
-                Id::from(ExtendedId::new_unchecked(id))
+                embedded_can::ExtendedId::new(id).unwrap().into()
             };
-            let data_len = fifo.rdtr().read().dlc() as usize;
+            let data_len = fifo.rdtr().read().dlc();
             let mut data: [u8; 8] = [0; 8];
             data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes());
             data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes());
 
-            let frame = Frame::new_data(id, Data::new(&data[0..data_len]).unwrap());
+            let frame = Frame::new(Header::new(id, data_len, false), &data).unwrap();
             let envelope = Envelope {
                 #[cfg(feature = "time")]
                 ts,
diff --git a/embassy-stm32/src/can/enums.rs b/embassy-stm32/src/can/enums.rs
index 36139a45c..651de9194 100644
--- a/embassy-stm32/src/can/enums.rs
+++ b/embassy-stm32/src/can/enums.rs
@@ -28,3 +28,15 @@ pub enum BusError {
     ///  At least one of error counter has reached the Error_Warning limit of 96.
     BusWarning,
 }
+
+/// Frame Create Errors
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub enum FrameCreateError {
+    /// Data in header does not match supplied.
+    NotEnoughData,
+    /// Invalid data length not 0-8 for Classic packet or valid for FD.
+    InvalidDataLength,
+    /// Invalid ID.
+    InvalidCanId,
+}
diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs
index 8ec09ac12..e14977d91 100644
--- a/embassy-stm32/src/can/fd/peripheral.rs
+++ b/embassy-stm32/src/can/fd/peripheral.rs
@@ -56,7 +56,10 @@ impl Registers {
         match maybe_header {
             Some((header, ts)) => {
                 let data = &buffer[0..header.len() as usize];
-                Some((F::from_header(header, data)?, ts))
+                match F::from_header(header, data) {
+                    Ok(frame) => Some((frame, ts)),
+                    Err(_) => None,
+                }
             }
             None => None,
         }
@@ -182,7 +185,7 @@ impl Registers {
                 DataLength::Fdcan(len) => len,
                 DataLength::Classic(len) => len,
             };
-            if len as usize > ClassicFrame::MAX_DATA_LEN {
+            if len as usize > ClassicData::MAX_DATA_LEN {
                 return None;
             }
 
diff --git a/embassy-stm32/src/can/frame.rs b/embassy-stm32/src/can/frame.rs
index 9c293035d..14fc32c51 100644
--- a/embassy-stm32/src/can/frame.rs
+++ b/embassy-stm32/src/can/frame.rs
@@ -1,6 +1,8 @@
 //! Definition for CAN Frames
 use bit_field::BitField;
 
+use crate::can::enums::FrameCreateError;
+
 /// CAN Header, without meta data
 #[derive(Debug, Copy, Clone)]
 pub struct Header {
@@ -9,6 +11,20 @@ pub struct Header {
     flags: u8,
 }
 
+#[cfg(feature = "defmt")]
+impl defmt::Format for Header {
+    fn format(&self, fmt: defmt::Formatter<'_>) {
+        match self.id() {
+            embedded_can::Id::Standard(id) => {
+                defmt::write!(fmt, "Can Standard ID={:x} len={}", id.as_raw(), self.len,)
+            }
+            embedded_can::Id::Extended(id) => {
+                defmt::write!(fmt, "Can Extended ID={:x} len={}", id.as_raw(), self.len,)
+            }
+        }
+    }
+}
+
 impl Header {
     const FLAG_RTR: usize = 0; // Remote
     const FLAG_FDCAN: usize = 1; // FDCan vs Classic CAN
@@ -54,13 +70,21 @@ impl Header {
     pub fn bit_rate_switching(&self) -> bool {
         self.flags.get_bit(Self::FLAG_BRS)
     }
+
+    /// Get priority of frame
+    pub(crate) fn priority(&self) -> u32 {
+        match self.id() {
+            embedded_can::Id::Standard(id) => (id.as_raw() as u32) << 18,
+            embedded_can::Id::Extended(id) => id.as_raw(),
+        }
+    }
 }
 
 /// Trait for FDCAN frame types, providing ability to construct from a Header
 /// and to retrieve the Header from a frame
 pub trait CanHeader: Sized {
     /// Construct frame from header and payload
-    fn from_header(header: Header, data: &[u8]) -> Option<Self>;
+    fn from_header(header: Header, data: &[u8]) -> Result<Self, FrameCreateError>;
 
     /// Get this frame's header struct
     fn header(&self) -> &Header;
@@ -70,24 +94,26 @@ pub trait CanHeader: Sized {
 ///
 /// Contains 0 to 8 Bytes of data.
 #[derive(Debug, Copy, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 pub struct ClassicData {
-    pub(crate) bytes: [u8; 8],
+    pub(crate) bytes: [u8; Self::MAX_DATA_LEN],
 }
 
 impl ClassicData {
+    pub(crate) const MAX_DATA_LEN: usize = 8;
     /// 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;
+    pub fn new(data: &[u8]) -> Result<Self, FrameCreateError> {
+        if data.len() > 8 {
+            return Err(FrameCreateError::InvalidDataLength);
         }
 
         let mut bytes = [0; 8];
         bytes[..data.len()].copy_from_slice(data);
 
-        Some(Self { bytes })
+        Ok(Self { bytes })
     }
 
     /// Raw read access to data.
@@ -112,58 +138,50 @@ impl ClassicData {
 
 /// Frame with up to 8 bytes of data payload as per Classic CAN
 #[derive(Debug, Copy, Clone)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
 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 }
+    pub fn new(can_header: Header, raw_data: &[u8]) -> Result<Self, FrameCreateError> {
+        let data = ClassicData::new(raw_data)?;
+        Ok(ClassicFrame { can_header, data: data })
+    }
+
+    /// Creates a new data frame.
+    pub fn new_data(id: impl Into<embedded_can::Id>, data: &[u8]) -> Result<Self, FrameCreateError> {
+        let eid: embedded_can::Id = id.into();
+        let header = Header::new(eid, data.len() as u8, false);
+        Self::new(header, data)
     }
 
     /// Create new extended frame
-    pub fn new_extended(raw_id: u32, raw_data: &[u8]) -> Option<Self> {
+    pub fn new_extended(raw_id: u32, raw_data: &[u8]) -> Result<Self, FrameCreateError> {
         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,
-            }
+            Self::new(Header::new(id.into(), raw_data.len() as u8, false), raw_data)
         } else {
-            None
+            Err(FrameCreateError::InvalidCanId)
         }
     }
 
     /// Create new standard frame
-    pub fn new_standard(raw_id: u16, raw_data: &[u8]) -> Option<Self> {
+    pub fn new_standard(raw_id: u16, raw_data: &[u8]) -> Result<Self, FrameCreateError> {
         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,
-            }
+            Self::new(Header::new(id.into(), raw_data.len() as u8, false), raw_data)
         } else {
-            None
+            Err(FrameCreateError::InvalidCanId)
         }
     }
 
     /// Create new remote frame
-    pub fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Option<Self> {
+    pub fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Result<Self, FrameCreateError> {
         if len <= 8usize {
-            Some(ClassicFrame::new(
-                Header::new(id.into(), len as u8, true),
-                ClassicData::empty(),
-            ))
+            Self::new(Header::new(id.into(), len as u8, true), &[0; 8])
         } else {
-            None
+            Err(FrameCreateError::InvalidDataLength)
         }
     }
 
@@ -181,24 +199,28 @@ impl ClassicFrame {
     pub fn data(&self) -> &[u8] {
         &self.data.raw()
     }
+
+    /// Get priority of frame
+    pub fn priority(&self) -> u32 {
+        self.header().priority()
+    }
 }
 
 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,
+        let frameopt = ClassicFrame::new(Header::new(id.into(), raw_data.len() as u8, false), raw_data);
+        match frameopt {
+            Ok(frame) => Some(frame),
+            Err(_) => 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(),
-            ))
+            let frameopt = ClassicFrame::new(Header::new(id.into(), len as u8, true), &[0; 8]);
+            match frameopt {
+                Ok(frame) => Some(frame),
+                Err(_) => None,
+            }
         } else {
             None
         }
@@ -224,8 +246,8 @@ impl embedded_can::Frame for ClassicFrame {
 }
 
 impl CanHeader for ClassicFrame {
-    fn from_header(header: Header, data: &[u8]) -> Option<Self> {
-        Some(Self::new(header, ClassicData::new(data)?))
+    fn from_header(header: Header, data: &[u8]) -> Result<Self, FrameCreateError> {
+        Self::new(header, data)
     }
 
     fn header(&self) -> &Header {
@@ -246,15 +268,15 @@ impl FdData {
     ///
     /// 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> {
+    pub fn new(data: &[u8]) -> Result<Self, FrameCreateError> {
         if !FdData::is_valid_len(data.len()) {
-            return None;
+            return Err(FrameCreateError::InvalidDataLength);
         }
 
         let mut bytes = [0; 64];
         bytes[..data.len()].copy_from_slice(data);
 
-        Some(Self { bytes })
+        Ok(Self { bytes })
     }
 
     /// Raw read access to data.
@@ -293,40 +315,35 @@ pub struct FdFrame {
 
 impl FdFrame {
     /// Create a new CAN classic Frame
-    pub fn new(can_header: Header, data: FdData) -> FdFrame {
-        FdFrame { can_header, data }
+    pub fn new(can_header: Header, raw_data: &[u8]) -> Result<Self, FrameCreateError> {
+        let data = FdData::new(raw_data)?;
+        Ok(FdFrame { can_header, data })
     }
 
     /// Create new extended frame
-    pub fn new_extended(raw_id: u32, raw_data: &[u8]) -> Option<Self> {
+    pub fn new_extended(raw_id: u32, raw_data: &[u8]) -> Result<Self, FrameCreateError> {
         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,
-            }
+            Self::new(Header::new(id.into(), raw_data.len() as u8, false), raw_data)
         } else {
-            None
+            Err(FrameCreateError::InvalidCanId)
         }
     }
 
     /// Create new standard frame
-    pub fn new_standard(raw_id: u16, raw_data: &[u8]) -> Option<Self> {
+    pub fn new_standard(raw_id: u16, raw_data: &[u8]) -> Result<Self, FrameCreateError> {
         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,
-            }
+            Self::new(Header::new(id.into(), raw_data.len() as u8, false), raw_data)
         } else {
-            None
+            Err(FrameCreateError::InvalidCanId)
         }
     }
 
     /// Create new remote frame
-    pub fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Option<Self> {
+    pub fn new_remote(id: impl Into<embedded_can::Id>, len: usize) -> Result<Self, FrameCreateError> {
         if len <= 8 {
-            Some(FdFrame::new(Header::new(id.into(), len as u8, true), FdData::empty()))
+            Self::new(Header::new(id.into(), len as u8, true), &[0; 8])
         } else {
-            None
+            Err(FrameCreateError::InvalidDataLength)
         }
     }
 
@@ -348,20 +365,17 @@ impl FdFrame {
 
 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,
+        match FdFrame::new(Header::new_fd(id.into(), raw_data.len() as u8, false, true), raw_data) {
+            Ok(frame) => Some(frame),
+            Err(_) => 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(),
-            ))
+            match FdFrame::new(Header::new_fd(id.into(), len as u8, true, true), &[0; 64]) {
+                Ok(frame) => Some(frame),
+                Err(_) => None,
+            }
         } else {
             None
         }
@@ -388,8 +402,8 @@ impl embedded_can::Frame for FdFrame {
 }
 
 impl CanHeader for FdFrame {
-    fn from_header(header: Header, data: &[u8]) -> Option<Self> {
-        Some(Self::new(header, FdData::new(data)?))
+    fn from_header(header: Header, data: &[u8]) -> Result<Self, FrameCreateError> {
+        Self::new(header, data)
     }
 
     fn header(&self) -> &Header {
diff --git a/examples/stm32f1/src/bin/can.rs b/examples/stm32f1/src/bin/can.rs
index 00d61096f..ac337e8a0 100644
--- a/examples/stm32f1/src/bin/can.rs
+++ b/examples/stm32f1/src/bin/can.rs
@@ -46,16 +46,16 @@ async fn main(_spawner: Spawner) {
 
     let mut i: u8 = 0;
     loop {
-        let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i, 0, 1, 2, 3, 4, 5, 6]);
+        let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), &[i, 0, 1, 2, 3, 4, 5, 6]).unwrap();
         can.write(&tx_frame).await;
 
         match can.read().await {
             Ok(env) => match env.frame.id() {
                 Id::Extended(id) => {
-                    defmt::println!("Extended Frame id={:x} {:02x}", id.as_raw(), env.frame.data().unwrap());
+                    defmt::println!("Extended Frame id={:x} {:02x}", id.as_raw(), env.frame.data());
                 }
                 Id::Standard(id) => {
-                    defmt::println!("Standard Frame id={:x} {:02x}", id.as_raw(), env.frame.data().unwrap());
+                    defmt::println!("Standard Frame id={:x} {:02x}", id.as_raw(), env.frame.data());
                 }
             },
             Err(err) => {
diff --git a/examples/stm32f4/src/bin/can.rs b/examples/stm32f4/src/bin/can.rs
index b20af8cf1..71b9453eb 100644
--- a/examples/stm32f4/src/bin/can.rs
+++ b/examples/stm32f4/src/bin/can.rs
@@ -51,7 +51,7 @@ async fn main(_spawner: Spawner) {
 
     let mut i: u8 = 0;
     loop {
-        let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i]);
+        let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), &[i]).unwrap();
         let tx_ts = Instant::now();
         can.write(&tx_frame).await;
 
@@ -65,7 +65,7 @@ async fn main(_spawner: Spawner) {
 
         info!(
             "loopback frame {=u8}, latency: {} us",
-            unwrap!(envelope.frame.data())[0],
+            envelope.frame.data()[0],
             latency.as_micros()
         );
         i += 1;
diff --git a/examples/stm32f7/src/bin/can.rs b/examples/stm32f7/src/bin/can.rs
index c3e14bbf4..221ac2a05 100644
--- a/examples/stm32f7/src/bin/can.rs
+++ b/examples/stm32f7/src/bin/can.rs
@@ -26,7 +26,7 @@ bind_interrupts!(struct Irqs {
 #[embassy_executor::task]
 pub async fn send_can_message(tx: &'static mut CanTx<'static, CAN3>) {
     loop {
-        let frame = Frame::new_data(unwrap!(StandardId::new(0 as _)), [0]);
+        let frame = Frame::new_data(unwrap!(StandardId::new(0 as _)), &[0]).unwrap();
         tx.write(&frame).await;
         embassy_time::Timer::after_secs(1).await;
     }
diff --git a/tests/stm32/src/bin/can.rs b/tests/stm32/src/bin/can.rs
index e869e8fb9..c08c69a3b 100644
--- a/tests/stm32/src/bin/can.rs
+++ b/tests/stm32/src/bin/can.rs
@@ -60,7 +60,7 @@ async fn main(_spawner: Spawner) {
 
     let mut i: u8 = 0;
     loop {
-        let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i]);
+        let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), &[i]).unwrap();
 
         info!("Transmitting frame...");
         let tx_ts = Instant::now();
@@ -70,7 +70,7 @@ async fn main(_spawner: Spawner) {
         info!("Frame received!");
 
         info!("loopback time {}", envelope.ts);
-        info!("loopback frame {=u8}", envelope.frame.data().unwrap()[0]);
+        info!("loopback frame {=u8}", envelope.frame.data()[0]);
 
         let latency = envelope.ts.saturating_duration_since(tx_ts);
         info!("loopback latency {} us", latency.as_micros());