stm32/can: implement proper RX timestamps

4 files changed, 58 insertions, 16 deletions

diff --git a/embassy-stm32/src/can/bxcan.rs b/embassy-stm32/src/can/bxcan.rs
index 8b8244d4f..795becabf 100644
--- a/embassy-stm32/src/can/bxcan.rs
+++ b/embassy-stm32/src/can/bxcan.rs
@@ -16,6 +16,17 @@ use crate::rcc::RccPeripheral;
 use crate::time::Hertz;
 use crate::{interrupt, peripherals, Peripheral};
 
+/// Contains CAN frame and additional metadata.
+///
+/// Timestamp is available if `time` feature is enabled.
+#[derive(Debug)]
+#[cfg_attr(feature = "defmt", derive(defmt::Format))]
+pub struct Envelope {
+    #[cfg(feature = "time")]
+    pub ts: embassy_time::Instant,
+    pub frame: bxcan::Frame,
+}
+
 /// Interrupt handler.
 pub struct TxInterruptHandler<T: Instance> {
     _phantom: PhantomData<T>,
@@ -199,11 +210,11 @@ impl<'d, T: Instance> Can<'d, T> {
     }
 
     /// Returns a tuple of the time the message was received and the message frame
-    pub async fn read(&mut self) -> Result<(u16, bxcan::Frame), BusError> {
+    pub async fn read(&mut self) -> Result<Envelope, BusError> {
         poll_fn(|cx| {
             T::state().err_waker.register(cx.waker());
-            if let Poll::Ready((time, frame)) = T::state().rx_queue.recv().poll_unpin(cx) {
-                return Poll::Ready(Ok((time, frame)));
+            if let Poll::Ready(envelope) = T::state().rx_queue.recv().poll_unpin(cx) {
+                return Poll::Ready(Ok(envelope));
             } else if let Some(err) = self.curr_error() {
                 return Poll::Ready(Err(err));
             }
@@ -228,6 +239,10 @@ impl<'d, T: Instance> Can<'d, T> {
     }
 
     unsafe fn receive_fifo(fifo: RxFifo) {
+        // Generate timestamp as early as possible
+        #[cfg(feature = "time")]
+        let ts = embassy_time::Instant::now();
+
         let state = T::state();
         let regs = T::regs();
         let fifo_idx = match fifo {
@@ -257,15 +272,19 @@ impl<'d, T: Instance> Can<'d, T> {
             data[0..4].copy_from_slice(&fifo.rdlr().read().0.to_ne_bytes());
             data[4..8].copy_from_slice(&fifo.rdhr().read().0.to_ne_bytes());
 
-            let time = fifo.rdtr().read().time();
             let frame = Frame::new_data(id, Data::new(&data[0..data_len]).unwrap());
+            let envelope = Envelope {
+                #[cfg(feature = "time")]
+                ts,
+                frame,
+            };
 
             rfr.modify(|v| v.set_rfom(true));
 
             /*
                 NOTE: consensus was reached that if rx_queue is full, packets should be dropped
             */
-            let _ = state.rx_queue.try_send((time, frame));
+            let _ = state.rx_queue.try_send(envelope);
         }
     }
 
@@ -405,11 +424,11 @@ pub struct CanRx<'c, 'd, T: Instance> {
 }
 
 impl<'c, 'd, T: Instance> CanRx<'c, 'd, T> {
-    pub async fn read(&mut self) -> Result<(u16, bxcan::Frame), BusError> {
+    pub async fn read(&mut self) -> Result<Envelope, BusError> {
         poll_fn(|cx| {
             T::state().err_waker.register(cx.waker());
-            if let Poll::Ready((time, frame)) = T::state().rx_queue.recv().poll_unpin(cx) {
-                return Poll::Ready(Ok((time, frame)));
+            if let Poll::Ready(envelope) = T::state().rx_queue.recv().poll_unpin(cx) {
+                return Poll::Ready(Ok(envelope));
             } else if let Some(err) = self.curr_error() {
                 return Poll::Ready(Err(err));
             }
@@ -467,10 +486,12 @@ pub(crate) mod sealed {
     use embassy_sync::channel::Channel;
     use embassy_sync::waitqueue::AtomicWaker;
 
+    use super::Envelope;
+
     pub struct State {
         pub tx_waker: AtomicWaker,
         pub err_waker: AtomicWaker,
-        pub rx_queue: Channel<CriticalSectionRawMutex, (u16, bxcan::Frame), 32>,
+        pub rx_queue: Channel<CriticalSectionRawMutex, Envelope, 32>,
     }
 
     impl State {
diff --git a/examples/stm32f4/src/bin/can.rs b/examples/stm32f4/src/bin/can.rs
index f84f74d30..20ce4edce 100644
--- a/examples/stm32f4/src/bin/can.rs
+++ b/examples/stm32f4/src/bin/can.rs
@@ -10,6 +10,7 @@ use embassy_stm32::can::bxcan::{Fifo, Frame, StandardId};
 use embassy_stm32::can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler};
 use embassy_stm32::gpio::{Input, Pull};
 use embassy_stm32::peripherals::CAN1;
+use embassy_time::Instant;
 use {defmt_rtt as _, panic_probe as _};
 
 bind_interrupts!(struct Irqs {
@@ -51,9 +52,22 @@ async fn main(_spawner: Spawner) {
     let mut i: u8 = 0;
     loop {
         let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i]);
+        let tx_ts = Instant::now();
         can.write(&tx_frame).await;
-        let (_, rx_frame) = can.read().await.unwrap();
-        info!("loopback frame {=u8}", unwrap!(rx_frame.data())[0]);
+
+        let envelope = can.read().await.unwrap();
+
+        // We can measure loopback latency by using receive timestamp in the `Envelope`.
+        // Our frame is ~55 bits long (exlcuding bit stuffing), so at 1mbps loopback delay is at least 55 us.
+        // When measured with `tick-hz-1_000_000` actual latency is 80~83 us, giving a combined hardware and software
+        // overhead of ~25 us. Note that CPU frequency can greatly affect the result.
+        let latency = envelope.ts.saturating_duration_since(tx_ts);
+
+        info!(
+            "loopback frame {=u8}, latency: {} us",
+            unwrap!(envelope.frame.data())[0],
+            latency.as_micros()
+        );
         i += 1;
     }
 }
diff --git a/examples/stm32f7/src/bin/can.rs b/examples/stm32f7/src/bin/can.rs
index 1b5b377ea..e9650f23a 100644
--- a/examples/stm32f7/src/bin/can.rs
+++ b/examples/stm32f7/src/bin/can.rs
@@ -60,7 +60,7 @@ async fn main(spawner: Spawner) {
     spawner.spawn(send_can_message(tx)).unwrap();
 
     loop {
-        let frame = rx.read().await.unwrap();
-        println!("Received: {:?}", frame);
+        let envelope = rx.read().await.unwrap();
+        println!("Received: {:?}", envelope);
     }
 }
diff --git a/tests/stm32/src/bin/can.rs b/tests/stm32/src/bin/can.rs
index 8bdd3c24f..93253ab84 100644
--- a/tests/stm32/src/bin/can.rs
+++ b/tests/stm32/src/bin/can.rs
@@ -7,6 +7,7 @@
 #[path = "../common.rs"]
 mod common;
 use common::*;
+use defmt::assert;
 use embassy_executor::Spawner;
 use embassy_stm32::bind_interrupts;
 use embassy_stm32::can::bxcan::filter::Mask32;
@@ -14,6 +15,7 @@ use embassy_stm32::can::bxcan::{Fifo, Frame, StandardId};
 use embassy_stm32::can::{Can, Rx0InterruptHandler, Rx1InterruptHandler, SceInterruptHandler, TxInterruptHandler};
 use embassy_stm32::gpio::{Input, Pull};
 use embassy_stm32::peripherals::CAN1;
+use embassy_time::{Duration, Instant};
 use {defmt_rtt as _, panic_probe as _};
 
 bind_interrupts!(struct Irqs {
@@ -62,13 +64,18 @@ async fn main(_spawner: Spawner) {
         let tx_frame = Frame::new_data(unwrap!(StandardId::new(i as _)), [i]);
 
         info!("Transmitting frame...");
+        let tx_ts = Instant::now();
         can.write(&tx_frame).await;
 
         info!("Receiving frame...");
-        let (time, rx_frame) = can.read().await.unwrap();
+        let envelope = can.read().await.unwrap();
 
-        info!("loopback time {}", time);
-        info!("loopback frame {=u8}", rx_frame.data().unwrap()[0]);
+        info!("loopback time {}", envelope.ts);
+        info!("loopback frame {=u8}", envelope.frame.data().unwrap()[0]);
+
+        // Theoretical minimum latency is 55us, actual is usually ~80us
+        let latency = envelope.ts.saturating_duration_since(tx_ts);
+        assert!(Duration::from_micros(50) < latency && latency < Duration::from_micros(100));
 
         i += 1;
         if i > 10 {