From 5ad291b708528b5772d6ebcc9309fbd3f8a002c8 Mon Sep 17 00:00:00 2001 From: Corey Schuhen Date: Sat, 17 Feb 2024 18:12:47 +1000 Subject: [PATCH] Add a buffered mode. --- embassy-stm32/src/can/fdcan.rs | 233 ++++++++++++++++++++++++++++++++ examples/stm32g4/src/bin/can.rs | 97 ++++++++++++- 2 files changed, 323 insertions(+), 7 deletions(-) diff --git a/embassy-stm32/src/can/fdcan.rs b/embassy-stm32/src/can/fdcan.rs index 42ce73ded..3ae330e14 100644 --- a/embassy-stm32/src/can/fdcan.rs +++ b/embassy-stm32/src/can/fdcan.rs @@ -5,6 +5,8 @@ use core::task::Poll; pub mod fd; use embassy_hal_internal::{into_ref, PeripheralRef}; +use embassy_sync::blocking_mutex::raw::CriticalSectionRawMutex; +use embassy_sync::channel::Channel; use fd::config::*; use fd::filter::*; @@ -49,6 +51,26 @@ impl interrupt::typelevel::Handler for IT0Interrup match &T::state().tx_mode { sealed::TxMode::NonBuffered(waker) => waker.wake(), + sealed::TxMode::ClassicBuffered(buf) => { + if !T::registers().tx_queue_is_full() { + match buf.tx_receiver.try_receive() { + Ok(frame) => { + _ = T::registers().write_classic(&frame); + } + Err(_) => {} + } + } + } + sealed::TxMode::FdBuffered(buf) => { + if !T::registers().tx_queue_is_full() { + match buf.tx_receiver.try_receive() { + Ok(frame) => { + _ = T::registers().write_fd(&frame); + } + Err(_) => {} + } + } + } } } @@ -405,6 +427,179 @@ where ) } + /// Return a buffered instance of driver without CAN FD support. User must supply Buffers + pub fn buffered( + &self, + tx_buf: &'static mut TxBuf, + rxb: &'static mut RxBuf, + ) -> BufferedCan<'d, T, M, TX_BUF_SIZE, RX_BUF_SIZE> { + BufferedCan::new(PhantomData::, T::regs(), self._mode, tx_buf, rxb) + } + + /// Return a buffered instance of driver with CAN FD support. User must supply Buffers + pub fn buffered_fd( + &self, + tx_buf: &'static mut TxFdBuf, + rxb: &'static mut RxFdBuf, + ) -> BufferedCanFd<'d, T, M, TX_BUF_SIZE, RX_BUF_SIZE> { + BufferedCanFd::new(PhantomData::, T::regs(), self._mode, tx_buf, rxb) + } +} + +/// User supplied buffer for RX Buffering +pub type RxBuf = Channel; + +/// User supplied buffer for TX buffering +pub type TxBuf = Channel; + +/// Buffered FDCAN Instance +#[allow(dead_code)] +pub struct BufferedCan<'d, T: Instance, M: FdcanOperatingMode, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> { + _instance1: PhantomData, + _instance2: &'d crate::pac::can::Fdcan, + _mode: PhantomData, + tx_buf: &'static TxBuf, + rx_buf: &'static RxBuf, +} + +impl<'c, 'd, T: Instance, M: Transmit, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> + BufferedCan<'d, T, M, TX_BUF_SIZE, RX_BUF_SIZE> +where + M: FdcanOperatingMode, +{ + fn new( + _instance1: PhantomData, + _instance2: &'d crate::pac::can::Fdcan, + _mode: PhantomData, + tx_buf: &'static TxBuf, + rx_buf: &'static RxBuf, + ) -> Self { + BufferedCan { + _instance1, + _instance2, + _mode, + tx_buf, + rx_buf, + } + .setup() + } + + fn setup(self) -> Self { + // We don't want interrupts being processed while we change modes. + critical_section::with(|_| unsafe { + let rx_inner = sealed::ClassicBufferedRxInner { + rx_sender: self.rx_buf.sender().into(), + }; + let tx_inner = sealed::ClassicBufferedTxInner { + tx_receiver: self.tx_buf.receiver().into(), + }; + T::mut_state().rx_mode = sealed::RxMode::ClassicBuffered(rx_inner); + T::mut_state().tx_mode = sealed::TxMode::ClassicBuffered(tx_inner); + }); + self + } + + /// Async write frame to TX buffer. + pub async fn write(&mut self, frame: ClassicFrame) { + self.tx_buf.send(frame).await; + T::IT0Interrupt::pend(); // Wake for Tx + } + + /// Async read frame from RX buffer. + pub async fn read(&mut self) -> Result<(ClassicFrame, Timestamp), BusError> { + Ok(self.rx_buf.receive().await) + } +} + +impl<'c, 'd, T: Instance, M, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> Drop + for BufferedCan<'d, T, M, TX_BUF_SIZE, RX_BUF_SIZE> +where + M: FdcanOperatingMode, +{ + fn drop(&mut self) { + critical_section::with(|_| unsafe { + T::mut_state().rx_mode = sealed::RxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new()); + T::mut_state().tx_mode = sealed::TxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new()); + }); + } +} + +/// User supplied buffer for RX Buffering +pub type RxFdBuf = Channel; + +/// User supplied buffer for TX buffering +pub type TxFdBuf = Channel; + +/// Buffered FDCAN Instance +#[allow(dead_code)] +pub struct BufferedCanFd<'d, T: Instance, M: FdcanOperatingMode, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> { + _instance1: PhantomData, + _instance2: &'d crate::pac::can::Fdcan, + _mode: PhantomData, + tx_buf: &'static TxFdBuf, + rx_buf: &'static RxFdBuf, +} + +impl<'c, 'd, T: Instance, M: Transmit, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> + BufferedCanFd<'d, T, M, TX_BUF_SIZE, RX_BUF_SIZE> +where + M: FdcanOperatingMode, +{ + fn new( + _instance1: PhantomData, + _instance2: &'d crate::pac::can::Fdcan, + _mode: PhantomData, + tx_buf: &'static TxFdBuf, + rx_buf: &'static RxFdBuf, + ) -> Self { + BufferedCanFd { + _instance1, + _instance2, + _mode, + tx_buf, + rx_buf, + } + .setup() + } + + fn setup(self) -> Self { + // We don't want interrupts being processed while we change modes. + critical_section::with(|_| unsafe { + let rx_inner = sealed::FdBufferedRxInner { + rx_sender: self.rx_buf.sender().into(), + }; + let tx_inner = sealed::FdBufferedTxInner { + tx_receiver: self.tx_buf.receiver().into(), + }; + T::mut_state().rx_mode = sealed::RxMode::FdBuffered(rx_inner); + T::mut_state().tx_mode = sealed::TxMode::FdBuffered(tx_inner); + }); + self + } + + /// Async write frame to TX buffer. + pub async fn write(&mut self, frame: FdFrame) { + self.tx_buf.send(frame).await; + T::IT0Interrupt::pend(); // Wake for Tx + } + + /// Async read frame from RX buffer. + pub async fn read(&mut self) -> Result<(FdFrame, Timestamp), BusError> { + Ok(self.rx_buf.receive().await) + } +} + +impl<'c, 'd, T: Instance, M, const TX_BUF_SIZE: usize, const RX_BUF_SIZE: usize> Drop + for BufferedCanFd<'d, T, M, TX_BUF_SIZE, RX_BUF_SIZE> +where + M: FdcanOperatingMode, +{ + fn drop(&mut self) { + critical_section::with(|_| unsafe { + T::mut_state().rx_mode = sealed::RxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new()); + T::mut_state().tx_mode = sealed::TxMode::NonBuffered(embassy_sync::waitqueue::AtomicWaker::new()); + }); + } } /// FDCAN Rx only Instance @@ -456,18 +651,39 @@ pub(crate) mod sealed { use core::future::poll_fn; use core::task::Poll; + use embassy_sync::channel::{DynamicReceiver, DynamicSender}; use embassy_sync::waitqueue::AtomicWaker; use crate::can::_version::{BusError, Timestamp}; use crate::can::frame::{ClassicFrame, FdFrame}; + + pub struct ClassicBufferedRxInner { + pub rx_sender: DynamicSender<'static, (ClassicFrame, Timestamp)>, + } + pub struct ClassicBufferedTxInner { + pub tx_receiver: DynamicReceiver<'static, ClassicFrame>, + } + + pub struct FdBufferedRxInner { + pub rx_sender: DynamicSender<'static, (FdFrame, Timestamp)>, + } + pub struct FdBufferedTxInner { + pub tx_receiver: DynamicReceiver<'static, FdFrame>, + } + pub enum RxMode { NonBuffered(AtomicWaker), + ClassicBuffered(ClassicBufferedRxInner), + FdBuffered(FdBufferedRxInner), } impl RxMode { pub fn register(&self, arg: &core::task::Waker) { match self { RxMode::NonBuffered(waker) => waker.register(arg), + _ => { + panic!("Bad Mode") + } } } @@ -477,6 +693,18 @@ pub(crate) mod sealed { RxMode::NonBuffered(waker) => { waker.wake(); } + RxMode::ClassicBuffered(buf) => { + if let Some(r) = T::registers().read_classic(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) { + let ts = T::calc_timestamp(T::state().ns_per_timer_tick, r.1); + let _ = buf.rx_sender.try_send((r.0, ts)); + } + } } } @@ -523,6 +751,8 @@ pub(crate) mod sealed { pub enum TxMode { NonBuffered(AtomicWaker), + ClassicBuffered(ClassicBufferedTxInner), + FdBuffered(FdBufferedTxInner), } impl TxMode { @@ -531,6 +761,9 @@ pub(crate) mod sealed { TxMode::NonBuffered(waker) => { waker.register(arg); } + _ => { + panic!("Bad mode"); + } } } diff --git a/examples/stm32g4/src/bin/can.rs b/examples/stm32g4/src/bin/can.rs index 5ff4f0ef0..043ca7144 100644 --- a/examples/stm32g4/src/bin/can.rs +++ b/examples/stm32g4/src/bin/can.rs @@ -5,6 +5,7 @@ use embassy_executor::Spawner; use embassy_stm32::peripherals::*; use embassy_stm32::{bind_interrupts, can, Config}; use embassy_time::Timer; +use static_cell::StaticCell; use {defmt_rtt as _, panic_probe as _}; bind_interrupts!(struct Irqs { @@ -28,13 +29,17 @@ async fn main(_spawner: Spawner) { // 250k bps can.set_bitrate(250_000); + let use_fd = false; + // 1M bps - can.set_fd_data_bitrate(1_000_000, false); + if use_fd { + can.set_fd_data_bitrate(1_000_000, false); + } info!("Configured"); - //let mut can = can.into_normal_mode(); - let mut can = can.into_internal_loopback_mode(); + let mut can = can.into_normal_mode(); + //let mut can = can.into_internal_loopback_mode(); let mut i = 0; let mut last_read_ts = embassy_time::Instant::now(); @@ -68,11 +73,17 @@ async fn main(_spawner: Spawner) { } // Use the FD API's even if we don't get FD packets. - loop { - let frame = can::frame::FdFrame::new_extended(0x123456F, &[i; 16]).unwrap(); - info!("Writing frame using FD API"); - _ = can.write_fd(&frame).await; + loop { + if use_fd { + let frame = can::frame::FdFrame::new_extended(0x123456F, &[i; 16]).unwrap(); + info!("Writing frame using FD API"); + _ = can.write_fd(&frame).await; + } else { + let frame = can::frame::FdFrame::new_extended(0x123456F, &[i; 8]).unwrap(); + info!("Writing frame using FD API"); + _ = can.write_fd(&frame).await; + } match can.read_fd().await { Ok((rx_frame, ts)) => { @@ -96,6 +107,7 @@ async fn main(_spawner: Spawner) { } } + i = 0; let (mut tx, mut rx) = can.split(); // With split loop { @@ -120,5 +132,76 @@ async fn main(_spawner: Spawner) { Timer::after_millis(250).await; i += 1; + + if i > 2 { + break; + } + } + + let can = can::Fdcan::join(tx, rx); + + info!("\n\n\nBuffered\n"); + if use_fd { + static TX_BUF: StaticCell> = StaticCell::new(); + static RX_BUF: StaticCell> = StaticCell::new(); + let mut can = can.buffered_fd( + TX_BUF.init(can::TxFdBuf::<8>::new()), + RX_BUF.init(can::RxFdBuf::<10>::new()), + ); + loop { + let frame = can::frame::FdFrame::new_extended(0x123456F, &[i; 16]).unwrap(); + info!("Writing frame"); + + _ = can.write(frame).await; + + match can.read().await { + Ok((rx_frame, ts)) => { + let delta = (ts - last_read_ts).as_millis(); + last_read_ts = ts; + info!( + "Rx: {} {:02x} --- {}ms", + rx_frame.header().len(), + rx_frame.data()[0..rx_frame.header().len() as usize], + delta, + ) + } + Err(_err) => error!("Error in frame"), + } + + Timer::after_millis(250).await; + + i += 1; + } + } else { + static TX_BUF: StaticCell> = StaticCell::new(); + static RX_BUF: StaticCell> = StaticCell::new(); + let mut can = can.buffered( + TX_BUF.init(can::TxBuf::<8>::new()), + RX_BUF.init(can::RxBuf::<10>::new()), + ); + loop { + let frame = can::frame::ClassicFrame::new_extended(0x123456F, &[i; 8]).unwrap(); + info!("Writing frame"); + + _ = can.write(frame).await; + + match can.read().await { + Ok((rx_frame, ts)) => { + let delta = (ts - last_read_ts).as_millis(); + last_read_ts = ts; + info!( + "Rx: {} {:02x} --- {}ms", + rx_frame.header().len(), + rx_frame.data()[0..rx_frame.header().len() as usize], + delta, + ) + } + Err(_err) => error!("Error in frame"), + } + + Timer::after_millis(250).await; + + i += 1; + } } }