Merge pull request #4017 from shilga/SpinlockMutex

embassy-rp: Spinlock mutex implementation

4 files changed, 171 insertions, 42 deletions

diff --git a/embassy-rp/src/critical_section_impl.rs b/embassy-rp/src/critical_section_impl.rs
index d233e6fab..2e4e8f716 100644
--- a/embassy-rp/src/critical_section_impl.rs
+++ b/embassy-rp/src/critical_section_impl.rs
@@ -1,6 +1,7 @@
 use core::sync::atomic::{AtomicU8, Ordering};
 
 use crate::pac;
+use crate::spinlock::Spinlock;
 
 struct RpSpinlockCs;
 critical_section::set_impl!(RpSpinlockCs);
@@ -92,46 +93,4 @@ impl RpSpinlockCs {
     }
 }
 
-pub struct Spinlock<const N: usize>(core::marker::PhantomData<()>)
-where
-    Spinlock<N>: SpinlockValid;
-
-impl<const N: usize> Spinlock<N>
-where
-    Spinlock<N>: SpinlockValid,
-{
-    /// Try to claim the spinlock. Will return `Some(Self)` if the lock is obtained, and `None` if the lock is
-    /// already in use somewhere else.
-    pub fn try_claim() -> Option<Self> {
-        let lock = pac::SIO.spinlock(N).read();
-        if lock > 0 {
-            Some(Self(core::marker::PhantomData))
-        } else {
-            None
-        }
-    }
-
-    /// Clear a locked spin-lock.
-    ///
-    /// # Safety
-    ///
-    /// Only call this function if you hold the spin-lock.
-    pub unsafe fn release() {
-        // Write (any value): release the lock
-        pac::SIO.spinlock(N).write_value(1);
-    }
-}
-
-impl<const N: usize> Drop for Spinlock<N>
-where
-    Spinlock<N>: SpinlockValid,
-{
-    fn drop(&mut self) {
-        // This is safe because we own the object, and hence hold the lock.
-        unsafe { Self::release() }
-    }
-}
-
 pub(crate) type Spinlock31 = Spinlock<31>;
-pub trait SpinlockValid {}
-impl SpinlockValid for Spinlock<31> {}
diff --git a/embassy-rp/src/lib.rs b/embassy-rp/src/lib.rs
index 35099d07b..f549446bc 100644
--- a/embassy-rp/src/lib.rs
+++ b/embassy-rp/src/lib.rs
@@ -41,6 +41,8 @@ pub mod rom_data;
 #[cfg(feature = "rp2040")]
 pub mod rtc;
 pub mod spi;
+mod spinlock;
+pub mod spinlock_mutex;
 #[cfg(feature = "time-driver")]
 pub mod time_driver;
 #[cfg(feature = "_rp235x")]
diff --git a/embassy-rp/src/spinlock.rs b/embassy-rp/src/spinlock.rs
new file mode 100644
index 000000000..7effd2ae0
--- /dev/null
+++ b/embassy-rp/src/spinlock.rs
@@ -0,0 +1,75 @@
+use crate::pac;
+
+pub struct Spinlock<const N: usize>(core::marker::PhantomData<()>)
+where
+    Spinlock<N>: SpinlockValid;
+
+impl<const N: usize> Spinlock<N>
+where
+    Spinlock<N>: SpinlockValid,
+{
+    /// Try to claim the spinlock. Will return `Some(Self)` if the lock is obtained, and `None` if the lock is
+    /// already in use somewhere else.
+    pub fn try_claim() -> Option<Self> {
+        let lock = pac::SIO.spinlock(N).read();
+        if lock > 0 {
+            Some(Self(core::marker::PhantomData))
+        } else {
+            None
+        }
+    }
+
+    /// Clear a locked spin-lock.
+    ///
+    /// # Safety
+    ///
+    /// Only call this function if you hold the spin-lock.
+    pub unsafe fn release() {
+        // Write (any value): release the lock
+        pac::SIO.spinlock(N).write_value(1);
+    }
+}
+
+impl<const N: usize> Drop for Spinlock<N>
+where
+    Spinlock<N>: SpinlockValid,
+{
+    fn drop(&mut self) {
+        // This is safe because we own the object, and hence hold the lock.
+        unsafe { Self::release() }
+    }
+}
+
+pub trait SpinlockValid {}
+impl SpinlockValid for Spinlock<0> {}
+impl SpinlockValid for Spinlock<1> {}
+impl SpinlockValid for Spinlock<2> {}
+impl SpinlockValid for Spinlock<3> {}
+impl SpinlockValid for Spinlock<4> {}
+impl SpinlockValid for Spinlock<5> {}
+impl SpinlockValid for Spinlock<6> {}
+impl SpinlockValid for Spinlock<7> {}
+impl SpinlockValid for Spinlock<8> {}
+impl SpinlockValid for Spinlock<9> {}
+impl SpinlockValid for Spinlock<10> {}
+impl SpinlockValid for Spinlock<11> {}
+impl SpinlockValid for Spinlock<12> {}
+impl SpinlockValid for Spinlock<13> {}
+impl SpinlockValid for Spinlock<14> {}
+impl SpinlockValid for Spinlock<15> {}
+impl SpinlockValid for Spinlock<16> {}
+impl SpinlockValid for Spinlock<17> {}
+impl SpinlockValid for Spinlock<18> {}
+impl SpinlockValid for Spinlock<19> {}
+impl SpinlockValid for Spinlock<20> {}
+impl SpinlockValid for Spinlock<21> {}
+impl SpinlockValid for Spinlock<22> {}
+impl SpinlockValid for Spinlock<23> {}
+impl SpinlockValid for Spinlock<24> {}
+impl SpinlockValid for Spinlock<25> {}
+impl SpinlockValid for Spinlock<26> {}
+impl SpinlockValid for Spinlock<27> {}
+impl SpinlockValid for Spinlock<28> {}
+impl SpinlockValid for Spinlock<29> {}
+impl SpinlockValid for Spinlock<30> {}
+impl SpinlockValid for Spinlock<31> {}
diff --git a/embassy-rp/src/spinlock_mutex.rs b/embassy-rp/src/spinlock_mutex.rs
new file mode 100644
index 000000000..85174cf86
--- /dev/null
+++ b/embassy-rp/src/spinlock_mutex.rs
@@ -0,0 +1,93 @@
+//! Mutex implementation utilizing an hardware spinlock
+
+use core::marker::PhantomData;
+use core::sync::atomic::Ordering;
+
+use embassy_sync::blocking_mutex::raw::RawMutex;
+
+use crate::spinlock::{Spinlock, SpinlockValid};
+
+/// A mutex that allows borrowing data across executors and interrupts by utilizing an hardware spinlock
+///
+/// # Safety
+///
+/// This mutex is safe to share between different executors and interrupts.
+pub struct SpinlockRawMutex<const N: usize> {
+    _phantom: PhantomData<()>,
+}
+unsafe impl<const N: usize> Send for SpinlockRawMutex<N> {}
+unsafe impl<const N: usize> Sync for SpinlockRawMutex<N> {}
+
+impl<const N: usize> SpinlockRawMutex<N> {
+    /// Create a new `SpinlockRawMutex`.
+    pub const fn new() -> Self {
+        Self { _phantom: PhantomData }
+    }
+}
+
+unsafe impl<const N: usize> RawMutex for SpinlockRawMutex<N>
+where
+    Spinlock<N>: SpinlockValid,
+{
+    const INIT: Self = Self::new();
+
+    fn lock<R>(&self, f: impl FnOnce() -> R) -> R {
+        // Store the initial interrupt state in stack variable
+        let interrupts_active = cortex_m::register::primask::read().is_active();
+
+        // Spin until we get the lock
+        loop {
+            // Need to disable interrupts to ensure that we will not deadlock
+            // if an interrupt or higher prio locks the spinlock after we acquire the lock
+            cortex_m::interrupt::disable();
+            // Ensure the compiler doesn't re-order accesses and violate safety here
+            core::sync::atomic::compiler_fence(Ordering::SeqCst);
+            if let Some(lock) = Spinlock::<N>::try_claim() {
+                // We just acquired the lock.
+                // 1. Forget it, so we don't immediately unlock
+                core::mem::forget(lock);
+                break;
+            }
+            // We didn't get the lock, enable interrupts if they were enabled before we started
+            if interrupts_active {
+                // safety: interrupts are only enabled, if they had been enabled before
+                unsafe {
+                    cortex_m::interrupt::enable();
+                }
+            }
+        }
+
+        let retval = f();
+
+        // Ensure the compiler doesn't re-order accesses and violate safety here
+        core::sync::atomic::compiler_fence(Ordering::SeqCst);
+        // Release the spinlock to allow others to lock mutex again
+        // safety: this point is only reached a spinlock was acquired before
+        unsafe {
+            Spinlock::<N>::release();
+        }
+
+        // Re-enable interrupts if they were enabled before the mutex was locked
+        if interrupts_active {
+            // safety: interrupts are only enabled, if they had been enabled before
+            unsafe {
+                cortex_m::interrupt::enable();
+            }
+        }
+
+        retval
+    }
+}
+
+pub mod blocking_mutex {
+    //! Mutex implementation utilizing an hardware spinlock
+    use embassy_sync::blocking_mutex::Mutex;
+
+    use crate::spinlock_mutex::SpinlockRawMutex;
+    /// A mutex that allows borrowing data across executors and interrupts by utilizing an hardware spinlock.
+    ///
+    /// # Safety
+    ///
+    /// This mutex is safe to share between different executors and interrupts.
+    pub type SpinlockMutex<const N: usize, T> = Mutex<SpinlockRawMutex<N>, T>;
+}