From 3f5c8784afe2174fd1df8446bd21240608c558e0 Mon Sep 17 00:00:00 2001
From: Corey Schuhen <cschuhen@gmail.com>
Date: Sat, 16 Mar 2024 19:24:47 +1000
Subject: [PATCH] FDCAN: Fix offset issue preventing CAN2 and CAN3 from
 working.

Fix for not H7
---
 embassy-stm32/src/can/fd/peripheral.rs |  7 +++-
 tests/stm32/src/bin/fdcan.rs           | 53 +++++++++++++++++++++++++-
 2 files changed, 57 insertions(+), 3 deletions(-)

diff --git a/embassy-stm32/src/can/fd/peripheral.rs b/embassy-stm32/src/can/fd/peripheral.rs
index e14977d91..682e13f4b 100644
--- a/embassy-stm32/src/can/fd/peripheral.rs
+++ b/embassy-stm32/src/can/fd/peripheral.rs
@@ -30,7 +30,12 @@ impl Registers {
         &mut self.msg_ram_mut().transmit.tbsa[bufidx]
     }
     pub fn msg_ram_mut(&self) -> &mut RegisterBlock {
+        #[cfg(stm32h7)]
+        let ptr = self.msgram.ram(self.msg_ram_offset / 4).as_ptr() as *mut RegisterBlock;
+
+        #[cfg(not(stm32h7))]
         let ptr = self.msgram.as_ptr() as *mut RegisterBlock;
+
         unsafe { &mut (*ptr) }
     }
 
@@ -637,7 +642,7 @@ impl Registers {
 
         use crate::can::fd::message_ram::*;
         //use fdcan::message_ram::*;
-        let mut offset_words = self.msg_ram_offset as u16;
+        let mut offset_words = (self.msg_ram_offset / 4) as u16;
 
         // 11-bit filter
         r.sidfc().modify(|w| w.set_flssa(offset_words));
diff --git a/tests/stm32/src/bin/fdcan.rs b/tests/stm32/src/bin/fdcan.rs
index dd78d7fb3..c7373e294 100644
--- a/tests/stm32/src/bin/fdcan.rs
+++ b/tests/stm32/src/bin/fdcan.rs
@@ -13,7 +13,11 @@ use embassy_stm32::{bind_interrupts, can, Config};
 use embassy_time::{Duration, Instant};
 use {defmt_rtt as _, panic_probe as _};
 
-bind_interrupts!(struct Irqs {
+bind_interrupts!(struct Irqs2 {
+    FDCAN2_IT0 => can::IT0InterruptHandler<FDCAN2>;
+    FDCAN2_IT1 => can::IT1InterruptHandler<FDCAN2>;
+});
+bind_interrupts!(struct Irqs1 {
     FDCAN1_IT0 => can::IT0InterruptHandler<FDCAN1>;
     FDCAN1_IT1 => can::IT1InterruptHandler<FDCAN1>;
 });
@@ -75,17 +79,24 @@ async fn main(_spawner: Spawner) {
     let options = options();
     let peripherals = embassy_stm32::init(options.config);
 
-    let mut can = can::FdcanConfigurator::new(peripherals.FDCAN1, peripherals.PB8, peripherals.PB9, Irqs);
+    let mut can = can::FdcanConfigurator::new(peripherals.FDCAN1, peripherals.PB8, peripherals.PB9, Irqs1);
+    let mut can2 = can::FdcanConfigurator::new(peripherals.FDCAN2, peripherals.PB12, peripherals.PB13, Irqs2);
 
     // 250k bps
     can.set_bitrate(250_000);
+    can2.set_bitrate(250_000);
 
     can.set_extended_filter(
         can::filter::ExtendedFilterSlot::_0,
         can::filter::ExtendedFilter::accept_all_into_fifo1(),
     );
+    can2.set_extended_filter(
+        can::filter::ExtendedFilterSlot::_0,
+        can::filter::ExtendedFilter::accept_all_into_fifo1(),
+    );
 
     let mut can = can.into_internal_loopback_mode();
+    let mut can2 = can2.into_internal_loopback_mode();
 
     info!("CAN Configured");
 
@@ -126,6 +137,44 @@ async fn main(_spawner: Spawner) {
         }
     }
 
+    let mut i: u8 = 0;
+    loop {
+        let tx_frame = can::frame::ClassicFrame::new_standard(0x123, &[i; 1]).unwrap();
+
+        info!("Transmitting frame...");
+        let tx_ts = Instant::now();
+        can2.write(&tx_frame).await;
+
+        let (frame, timestamp) = can2.read().await.unwrap();
+        info!("Frame received!");
+
+        //print_regs().await;
+        // Check data.
+        assert!(i == frame.data()[0], "{} == {}", i, frame.data()[0]);
+
+        info!("loopback time {}", timestamp);
+        info!("loopback frame {=u8}", frame.data()[0]);
+        let latency = timestamp.saturating_duration_since(tx_ts);
+        info!("loopback latency {} us", latency.as_micros());
+
+        // Theoretical minimum latency is 55us, actual is usually ~80us
+        const MIN_LATENCY: Duration = Duration::from_micros(50);
+        // Was failing at 150 but we are not getting a real time stamp. I'm not
+        // sure if there are other delays
+        assert!(
+            MIN_LATENCY <= latency && latency <= options.max_latency,
+            "{} <= {} <= {}",
+            MIN_LATENCY,
+            latency,
+            options.max_latency
+        );
+
+        i += 1;
+        if i > 10 {
+            break;
+        }
+    }
+
     let max_buffered = if options.second_fifo_working { 6 } else { 3 };
 
     // Below here, check that we can receive from both FIFO0 and FIFO0