NaxdyOrg/embassy
1
0
Fork 0
Code Issues Pull requests Projects Releases Packages Wiki Activity

Merge pull request #2635 from MaxiluxSystems/fdcan-refactor

Browse source 
stm32: can: fd: some refactoring
This commit is contained in:
Dario Nieuwenhuis 2024-02-28 00:50:40 +00:00 committed by GitHub
commit da4ccb62e4
No known key found for this signature in database
GPG key ID: B5690EEEBB952194
3 changed files with 69 additions and 195 deletions
Show stats Download patch file Download diff file Expand all files Collapse all files

166
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,43 +54,14 @@ 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,
}
}
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();
@ -193,11 +164,7 @@ 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);
@ -212,12 +179,14 @@ impl Registers {
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))
if header_reg.rtr().bit() {
F::new_remote(id, len as usize)
} else {
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
@ -227,52 +196,7 @@ impl Registers {
}
}
#[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> {
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 +205,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?
@ -557,8 +447,6 @@ impl Registers {
/// 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);
@ -571,8 +459,6 @@ impl Registers {
/// 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);
@ -590,7 +476,6 @@ impl Registers {
#[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
@ -598,21 +483,18 @@ impl Registers {
#[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
@ -632,16 +514,12 @@ impl Registers {
#[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
@ -665,8 +543,6 @@ impl Registers {
w.set_tcp(tcp);
w.set_tss(tss);
});
//self.control.config.timestamp_source = select;
}
#[cfg(not(stm32h7))]
@ -794,22 +670,6 @@ fn data_from_tx_buffer(buffer: &mut [u8], mailbox: &TxBufferElement, len: usize)
}
}
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();

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
}
}

32
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.
@ -272,8 +292,6 @@ pub struct FdFrame {
}
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 }
@ -368,3 +386,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()
}
}