LailaTheElf/embassy
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

stm32: can: fd: refactor out some duplicate code

Browse Source
This commit is contained in:
Torin Cooper-Bennun 2024-02-27 23:42:50 +00:00
parent 62c5df7e5b
commit 0ed402fd79
3 changed files with 68 additions and 150 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

120
embassy-stm32/src/can/fd/peripheral.rs
View File

@ -37,7 +37,7 @@ impl Registers {
&mut self.msg_ram_mut().receive[fifonr].fxsa[bufnum] &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? // Fill level - do we have a msg?
if self.regs.rxfs(fifonr).read().ffl() < 1 { if self.regs.rxfs(fifonr).read().ffl() < 1 {
return None; return None;
@ -54,32 +54,8 @@ impl Registers {
match maybe_header { match maybe_header {
Some((header, ts)) => { Some((header, ts)) => {
let data = ClassicData::new(&buffer[0..header.len() as usize]); let data = &buffer[0..header.len() as usize];
Some((ClassicFrame::new(header, data.unwrap()), ts)) Some((F::from_header(header, data)?, 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, None => None,
} }
@ -194,10 +170,9 @@ impl Registers {
#[inline] #[inline]
//fn abort_pending_mailbox<PTX, R>(&mut self, idx: Mailbox, pending: PTX) -> Option<R> //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 //where
// PTX: FnOnce(Mailbox, TxFrameHeader, &[u32]) -> R, // 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) { if self.abort(bufidx) {
let mailbox = self.tx_buffer_element(bufidx); let mailbox = self.tx_buffer_element(bufidx);
@ -216,50 +191,11 @@ impl Registers {
let mut data = [0u8; 64]; let mut data = [0u8; 64];
data_from_tx_buffer(&mut data, mailbox, len as usize); data_from_tx_buffer(&mut data, mailbox, len as usize);
let cd = ClassicData::new(&data).unwrap(); if header_reg.rtr().bit() {
Some(ClassicFrame::new(Header::new(id, len, header_reg.rtr().bit()), cd)) F::new_remote(id, len as usize)
} else { } else {
// Abort request failed because the frame was already sent (or being sent) on F::new(id, &data)
// 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 { } else {
// Abort request failed because the frame was already sent (or being sent) on // 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 // 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 /// As Transmit, but if there is a pending frame, `pending` will be called so that the frame can
/// be preserved. /// be preserved.
//pub fn transmit_preserve<PTX, P>( //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 queue_is_full = self.tx_queue_is_full();
let id = frame.header().id(); let id = frame.header().id();
@ -281,45 +217,11 @@ impl Registers {
// Discard the first slot with a lower priority message // Discard the first slot with a lower priority message
let (idx, pending_frame) = if queue_is_full { let (idx, pending_frame) = if queue_is_full {
if self.is_available(0, id) { 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) { } 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) { } else if self.is_available(2, id) {
(2, self.abort_pending_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?
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 { } else {
// For now we bail when there is no lower priority slot available // For now we bail when there is no lower priority slot available
// Can this lead to priority inversion? // Can this lead to priority inversion?

64
embassy-stm32/src/can/fdcan.rs
View File

@ -58,7 +58,7 @@ impl<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> for IT0Interrup
if !T::registers().tx_queue_is_full() { if !T::registers().tx_queue_is_full() {
match buf.tx_receiver.try_receive() { match buf.tx_receiver.try_receive() {
Ok(frame) => { Ok(frame) => {
_ = T::registers().write_classic(&frame); _ = T::registers().write(&frame);
} }
Err(_) => {} Err(_) => {}
} }
@ -68,7 +68,7 @@ impl<T: Instance> interrupt::typelevel::Handler<T::IT0Interrupt> for IT0Interrup
if !T::registers().tx_queue_is_full() { if !T::registers().tx_queue_is_full() {
match buf.tx_receiver.try_receive() { match buf.tx_receiver.try_receive() {
Ok(frame) => { Ok(frame) => {
_ = T::registers().write_fd(&frame); _ = T::registers().write(&frame);
} }
Err(_) => {} Err(_) => {}
} }
@ -359,7 +359,7 @@ impl<'d, T: Instance> Fdcan<'d, T> {
/// Returns the next received message frame /// Returns the next received message frame
pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> { 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 /// 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> { impl<'c, 'd, T: Instance> FdcanRx<'d, T> {
/// Returns the next received message frame /// Returns the next received message frame
pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> { 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 /// Returns the next received message frame
@ -649,6 +649,7 @@ pub(crate) mod sealed {
use embassy_sync::channel::{DynamicReceiver, DynamicSender}; use embassy_sync::channel::{DynamicReceiver, DynamicSender};
use embassy_sync::waitqueue::AtomicWaker; use embassy_sync::waitqueue::AtomicWaker;
use super::CanHeader;
use crate::can::_version::{BusError, Timestamp}; use crate::can::_version::{BusError, Timestamp};
use crate::can::frame::{ClassicFrame, FdFrame}; use crate::can::frame::{ClassicFrame, FdFrame};
@ -689,13 +690,13 @@ pub(crate) mod sealed {
waker.wake(); waker.wake();
} }
RxMode::ClassicBuffered(buf) => { 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 ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1);
let _ = buf.rx_sender.try_send((r.0, ts)); let _ = buf.rx_sender.try_send((r.0, ts));
} }
} }
RxMode::FdBuffered(buf) => { 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 ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1);
let _ = buf.rx_sender.try_send((r.0, ts)); 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| { poll_fn(|cx| {
T::state().err_waker.register(cx.waker()); T::state().err_waker.register(cx.waker());
self.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); let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts);
return Poll::Ready(Ok((msg, 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); let ts = T::calc_timestamp(T::state().ns_per_timer_tick, ts);
return Poll::Ready(Ok((msg, ts))); return Poll::Ready(Ok((msg, ts)));
} else if let Some(err) = T::registers().curr_error() { } else if let Some(err) = T::registers().curr_error() {
@ -723,24 +724,12 @@ pub(crate) mod sealed {
.await .await
} }
pub async fn read_classic<T: Instance>(&self) -> Result<(ClassicFrame, Timestamp), BusError> {
self.read::<T, _>().await
}
pub async fn read_fd<T: Instance>(&self) -> Result<(FdFrame, Timestamp), BusError> { pub async fn read_fd<T: Instance>(&self) -> Result<(FdFrame, Timestamp), BusError> {
poll_fn(|cx| { self.read::<T, _>().await
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
} }
} }
@ -766,11 +755,11 @@ pub(crate) mod sealed {
/// frame is dropped from the mailbox, it is returned. If no lower-priority frames /// 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 /// can be replaced, this call asynchronously waits for a frame to be successfully
/// transmitted, then tries again. /// 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| { poll_fn(|cx| {
self.register(cx.waker()); 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); return Poll::Ready(dropped);
} }
@ -785,19 +774,16 @@ pub(crate) mod sealed {
/// frame is dropped from the mailbox, it is returned. If no lower-priority frames /// 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 /// can be replaced, this call asynchronously waits for a frame to be successfully
/// transmitted, then tries again. /// transmitted, then tries again.
pub async fn write_fd<T: Instance>(&self, frame: &FdFrame) -> Option<FdFrame> { pub async fn write<T: Instance>(&self, frame: &ClassicFrame) -> Option<ClassicFrame> {
poll_fn(|cx| { self.write_generic::<T, _>(frame).await
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 /// Queues the message to be sent but exerts backpressure. If a lower-priority
// to clear. /// frame is dropped from the mailbox, it is returned. If no lower-priority frames
Poll::Pending /// can be replaced, this call asynchronously waits for a frame to be successfully
}) /// transmitted, then tries again.
.await pub async fn write_fd<T: Instance>(&self, frame: &FdFrame) -> Option<FdFrame> {
self.write_generic::<T, _>(frame).await
} }
} }

30
embassy-stm32/src/can/frame.rs
View File

@ -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. /// Payload of a classic CAN data frame.
/// ///
/// Contains 0 to 8 Bytes of data. /// 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. /// Payload of a (FD)CAN data frame.
/// ///
/// Contains 0 to 64 Bytes of data. /// Contains 0 to 64 Bytes of data.
@ -368,3 +388,13 @@ impl embedded_can::Frame for FdFrame {
&self.data.raw() &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()
}
}