diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs
index 9c29e4887..7d26a5fe0 100644
--- a/embassy-stm32/src/can/fd/peripheral.rs
+++ b/embassy-stm32/src/can/fd/peripheral.rs
@@ -37,7 +37,7 @@ impl Registers {
&mut self.msg_ram_mut().receive[fifonr].fxsa[bufnum]
}
- pub fn read_classic(&self, fifonr: usize) -> Option<(ClassicFrame, u16)> {
+ pub fn read<F: CanHeader>(&self, fifonr: usize) -> Option<(F, u16)> {
// Fill level - do we have a msg?
if self.regs.rxfs(fifonr).read().ffl() < 1 {
return None;
@@ -54,32 +54,8 @@ impl Registers {
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))
+ let data = &buffer[0..header.len() as usize];
+ Some((F::from_header(header, data)?, ts))
}
None => None,
}
@@ -194,10 +170,9 @@ impl Registers {
#[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,
- {
+ pub fn abort_pending_mailbox_generic<F: embedded_can::Frame>(&self, bufidx: usize) -> Option<F> {
if self.abort(bufidx) {
let mailbox = self.tx_buffer_element(bufidx);
@@ -216,50 +191,11 @@ impl Registers {
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())
+ if header_reg.rtr().bit() {
+ F::new_remote(id, len as usize)
} else {
- Header::new(id, len, header_reg.rtr().bit())
- };
-
- Some(FdFrame::new(header, cd))
+ F::new(id, &data)
+ }
} 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
@@ -272,7 +208,7 @@ impl Registers {
/// 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> {
+ pub fn write<F: embedded_can::Frame + CanHeader>(&self, frame: &F) -> nb::Result<Option<F>, Infallible> {
let queue_is_full = self.tx_queue_is_full();
let id = frame.header().id();
@@ -281,45 +217,11 @@ impl Registers {
// 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))
+ (0, self.abort_pending_mailbox_generic(0))
} else if self.is_available(1, id) {
- (1, self.abort_pending_mailbox(1))
+ (1, self.abort_pending_mailbox_generic(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))
+ (2, self.abort_pending_mailbox_generic(2))
} else {
// For now we bail when there is no lower priority slot available
// Can this lead to priority inversion?
diff --git a/embassy-stm32/src/can/fdcan.rs b/embassy-stm32/src/can/fdcan.rs
index 744d756f5..6a4a25cb7 100644
--- a/embassy-stm32/src/can/fdcan.rs
+++ b/embassy-stm32/src/can/fdcan.rs
@@ -58,7 +58,7 @@ impl<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> for IT0Interrup
if !T::registers().tx_queue_is_full() {
match buf.tx_receiver.try_receive() {
Ok(frame) => {
- _ = T::registers().write_classic(&frame);
+ _ = T::registers().write(&frame);
}
Err(_) => {}
}
@@ -68,7 +68,7 @@ impl<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> for IT0Interrup
if !T::registers().tx_queue_is_full() {
match buf.tx_receiver.try_receive() {
Ok(frame) => {
- _ = T::registers().write_fd(&frame);
+ _ = T::registers().write(&frame);
}
Err(_) => {}
}
@@ -359,7 +359,7 @@ impl<'d, T: Instance> Fdcan<'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
+ T::state().rx_mode.read_classic::<T>().await
}
/// Queues the message to be sent but exerts backpressure. If a lower-priority
@@ -633,7 +633,7 @@ impl<'c, 'd, T: Instance> FdcanTx<'d, T> {
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
+ T::state().rx_mode.read_classic::<T>().await
}
/// Returns the next received message frame
@@ -649,6 +649,7 @@ pub(crate) mod sealed {
use embassy_sync::channel::{DynamicReceiver, DynamicSender};
use embassy_sync::waitqueue::AtomicWaker;
+ use super::CanHeader;
use crate::can::_version::{BusError, Timestamp};
use crate::can::frame::{ClassicFrame, FdFrame};
@@ -689,13 +690,13 @@ pub(crate) mod sealed {
waker.wake();
}
RxMode::ClassicBuffered(buf) => {
- if let Some(r) = T::registers().read_classic(fifonr) {
+ if let Some(r) = T::registers().read(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) {
+ if let Some(r) = T::registers().read(fifonr) {
let ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1);
let _ = buf.rx_sender.try_send((r.0, ts));
}
@@ -703,15 +704,15 @@ pub(crate) mod sealed {
}
}
- pub async fn read<T: Instance>(&self) -> Result<(ClassicFrame, Timestamp), BusError> {
+ async fn read<T: Instance, F: CanHeader>(&self) -> Result<(F, 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) {
+ if let Some((msg, ts)) = T::registers().read(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) {
+ } else if let Some((msg, ts)) = T::registers().read(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() {
@@ -723,24 +724,12 @@ pub(crate) mod sealed {
.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());
+ pub async fn read_classic<T: Instance>(&self) -> Result<(ClassicFrame, Timestamp), BusError> {
+ self.read::<T, _>().await
+ }
- 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 async fn read_fd<T: Instance>(&self) -> Result<(FdFrame, Timestamp), BusError> {
+ self.read::<T, _>().await
}
}
@@ -766,11 +755,11 @@ pub(crate) mod sealed {
/// 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> {
+ async fn write_generic<T: Instance, F: embedded_can::Frame + CanHeader>(&self, frame: &F) -> Option<F> {
poll_fn(|cx| {
self.register(cx.waker());
- if let Ok(dropped) = T::registers().write_classic(frame) {
+ if let Ok(dropped) = T::registers().write(frame) {
return Poll::Ready(dropped);
}
@@ -781,23 +770,20 @@ pub(crate) mod sealed {
.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<T: Instance>(&self, frame: &ClassicFrame) -> Option<ClassicFrame> {
+ self.write_generic::<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<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
+ self.write_generic::<T, _>(frame).await
}
}
diff --git a/embassy-stm32/src/can/frame.rs b/embassy-stm32/src/can/frame.rs
index 725a9b1ab..59b9fb08c 100644
--- a/embassy-stm32/src/can/frame.rs
+++ b/embassy-stm32/src/can/frame.rs
@@ -56,6 +56,16 @@ impl Header {
}
}
+/// 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>;
+
+ /// Get this frame's header struct
+ fn header(&self) -> &Header;
+}
+
/// Payload of a classic CAN data frame.
///
/// Contains 0 to 8 Bytes of data.
@@ -213,6 +223,16 @@ 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 header(&self) -> &Header {
+ self.header()
+ }
+}
+
/// Payload of a (FD)CAN data frame.
///
/// Contains 0 to 64 Bytes of data.
@@ -368,3 +388,13 @@ impl embedded_can::Frame for FdFrame {
&self.data.raw()
}
}
+
+impl CanHeader for FdFrame {
+ fn from_header(header: Header, data: &[u8]) -> Option<Self> {
+ Some(Self::new(header, FdData::new(data)?))
+ }
+
+ fn header(&self) -> &Header {
+ self.header()
+ }
+}