2 files changed, 37 insertions, 5 deletions

diff --git a/embassy-hal-internal/src/interrupt.rs b/embassy-hal-internal/src/interrupt.rs
index b970aa2cd..19dabcf6f 100644
--- a/embassy-hal-internal/src/interrupt.rs
+++ b/embassy-hal-internal/src/interrupt.rs
@@ -4,6 +4,7 @@ use core::sync::atomic::{compiler_fence, Ordering};
 
 use cortex_m::interrupt::InterruptNumber;
 use cortex_m::peripheral::NVIC;
+use critical_section::CriticalSection;
 
 /// Generate a standard `mod interrupt` for a HAL.
 #[macro_export]
@@ -91,6 +92,12 @@ macro_rules! interrupt_mod {
                     fn set_priority(prio: crate::interrupt::Priority) {
                         Self::IRQ.set_priority(prio)
                     }
+
+                    /// Set the interrupt priority with an already-acquired critical section
+                    #[inline]
+                    fn set_priority_with_cs(cs: critical_section::CriticalSection, prio: crate::interrupt::Priority) {
+                        Self::IRQ.set_priority_with_cs(cs, prio)
+                    }
                 }
 
                 $(
@@ -195,10 +202,29 @@ pub unsafe trait InterruptExt: InterruptNumber + Copy {
     /// Set the interrupt priority.
     #[inline]
     fn set_priority(self, prio: Priority) {
-        critical_section::with(|_| unsafe {
+        unsafe {
+            let mut nvic: cortex_m::peripheral::NVIC = mem::transmute(());
+
+            // On thumbv6, set_priority must do a RMW to change 8bit in a 32bit reg.
+            #[cfg(armv6m)]
+            critical_section::with(|_| nvic.set_priority(self, prio.into()));
+            // On thumbv7+, set_priority does an atomic 8bit write, so no CS needed.
+            #[cfg(not(armv6m))]
+            nvic.set_priority(self, prio.into());
+        }
+    }
+
+    /// Set the interrupt priority with an already-acquired critical section
+    ///
+    /// Equivalent to `set_priority`, except you pass a `CriticalSection` to prove
+    /// you've already acquired a critical section. This prevents acquiring another
+    /// one, which saves code size.
+    #[inline]
+    fn set_priority_with_cs(self, _cs: CriticalSection, prio: Priority) {
+        unsafe {
             let mut nvic: cortex_m::peripheral::NVIC = mem::transmute(());
-            nvic.set_priority(self, prio.into())
-        })
+            nvic.set_priority(self, prio.into());
+        }
     }
 }
 
diff --git a/embassy-hal-internal/src/macros.rs b/embassy-hal-internal/src/macros.rs
index 0eea4b667..97df38954 100644
--- a/embassy-hal-internal/src/macros.rs
+++ b/embassy-hal-internal/src/macros.rs
@@ -48,17 +48,23 @@ macro_rules! peripherals_struct {
             ///Returns all the peripherals *once*
             #[inline]
             pub(crate) fn take() -> Self {
+                critical_section::with(Self::take_with_cs)
+            }
 
+            ///Returns all the peripherals *once*
+            #[inline]
+            pub(crate) fn take_with_cs(_cs: critical_section::CriticalSection) -> Self {
                 #[no_mangle]
                 static mut _EMBASSY_DEVICE_PERIPHERALS: bool = false;
 
-                critical_section::with(|_| unsafe {
+                // safety: OK because we're inside a CS.
+                unsafe {
                     if _EMBASSY_DEVICE_PERIPHERALS {
                         panic!("init called more than once!")
                     }
                     _EMBASSY_DEVICE_PERIPHERALS = true;
                     Self::steal()
-                })
+                }
             }
         }