Merge pull request #3851 from embassy-rs/rp23-atomics-fix

rp235x: fix atomics, fix SIO spinlock stuck bug, fix missing core1 reset.

2 files changed, 51 insertions, 12 deletions

diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index f0893b5a0..80ee47802 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -598,7 +598,7 @@ pub fn init(config: config::Config) -> Peripherals {
     peripherals
 }
 
-#[cfg(all(feature = "rt", feature = "rp2040"))]
+#[cfg(feature = "rt")]
 #[cortex_m_rt::pre_init]
 unsafe fn pre_init() {
     // SIO does not get reset when core0 is reset with either `scb::sys_reset()` or with SWD.
@@ -629,17 +629,52 @@ unsafe fn pre_init() {
     //
     // The PSM order is SIO -> PROC0 -> PROC1.
     // So, we have to force-on PROC0 to prevent it from getting reset when resetting SIO.
-    pac::PSM.frce_on().write_and_wait(|w| {
-        w.set_proc0(true);
-    });
-    // Then reset SIO and PROC1.
-    pac::PSM.frce_off().write_and_wait(|w| {
-        w.set_sio(true);
-        w.set_proc1(true);
-    });
-    // clear force_off first, force_on second. The other way around would reset PROC0.
-    pac::PSM.frce_off().write_and_wait(|_| {});
-    pac::PSM.frce_on().write_and_wait(|_| {});
+    #[cfg(feature = "rp2040")]
+    {
+        pac::PSM.frce_on().write_and_wait(|w| {
+            w.set_proc0(true);
+        });
+        // Then reset SIO and PROC1.
+        pac::PSM.frce_off().write_and_wait(|w| {
+            w.set_sio(true);
+            w.set_proc1(true);
+        });
+        // clear force_off first, force_on second. The other way around would reset PROC0.
+        pac::PSM.frce_off().write_and_wait(|_| {});
+        pac::PSM.frce_on().write_and_wait(|_| {});
+    }
+
+    #[cfg(feature = "_rp235x")]
+    {
+        // on RP235x, datasheet says "The FRCE_ON register is a development feature that does nothing in production devices."
+        // No idea why they removed it. Removing it means we can't use PSM to reset SIO, because it comes before
+        // PROC0, so we'd need FRCE_ON to prevent resetting ourselves.
+        //
+        // So we just unlock the spinlock manually.
+        pac::SIO.spinlock(31).write_value(1);
+
+        // We can still use PSM to reset PROC1 since it comes after PROC0 in the state machine.
+        pac::PSM.frce_off().write_and_wait(|w| w.set_proc1(true));
+        pac::PSM.frce_off().write_and_wait(|_| {});
+
+        // Make atomics work between cores.
+        enable_actlr_extexclall();
+    }
+}
+
+/// Set the EXTEXCLALL bit in ACTLR.
+///
+/// The default MPU memory map marks all memory as non-shareable, so atomics don't
+/// synchronize memory accesses between cores at all. This bit forces all memory to be
+/// considered shareable regardless of what the MPU says.
+///
+/// TODO: does this interfere somehow if the user wants to use a custom MPU configuration?
+/// maybe we need to add a way to disable this?
+///
+/// This must be done FOR EACH CORE.
+#[cfg(feature = "_rp235x")]
+unsafe fn enable_actlr_extexclall() {
+    (&*cortex_m::peripheral::ICB::PTR).actlr.modify(|w| w | (1 << 29));
 }
 
 /// Extension trait for PAC regs, adding atomic xor/bitset/bitclear writes.
diff --git a/embassy-rp/src/multicore.rs b/embassy-rp/src/multicore.rs
index ea0a29a36..1450505b9 100644
--- a/embassy-rp/src/multicore.rs
+++ b/embassy-rp/src/multicore.rs
@@ -65,6 +65,10 @@ unsafe fn core1_setup(stack_bottom: *mut usize) {
         // embassy, somehow. trap if so since we can't deal with that.
         cortex_m::asm::udf();
     }
+
+    #[cfg(feature = "_rp235x")]
+    crate::enable_actlr_extexclall();
+
     unsafe {
         gpio::init();
     }