NaxdyOrg/embassy
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages 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
embassy-stm32/src/can
fd
peripheral.rs
fdcan.rsframe.rs

122
embassy-stm32/src/can/fd/peripheral.rs
View file

@ -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?

66
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() {
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
}
}

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.
///
/// 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()
}
}