Optimize critical_section: use MSR instruction

cortex-m/Cargo.toml

@@ -33,7 +33,7 @@ cm7 = []
 cm7-r0p1 = ["cm7"]
 linker-plugin-lto = []
 std = []
-critical-section-single-core = ["critical-section/restore-state-bool"]
+critical-section-single-core = ["critical-section/restore-state-u32"]
 
 [package.metadata.docs.rs]
 targets = [

cortex-m/src/critical_section.rs

@@ -8,18 +8,17 @@ set_impl!(SingleCoreCriticalSection);
 
 unsafe impl Impl for SingleCoreCriticalSection {
     unsafe fn acquire() -> RawRestoreState {
-        let was_active = primask::read().is_active();
+        // Backup previous state of PRIMASK register. We access the entire register directly as a
+        // u32 instead of using the primask::read() function to minimize the number of processor
+        // cycles during which interrupts are disabled.
+        let restore_state = primask::read_raw();
         // NOTE: Fence guarantees are provided by interrupt::disable(), which performs a `compiler_fence(SeqCst)`.
         interrupt::disable();
-        was_active
+        restore_state
     }
 
-    unsafe fn release(was_active: RawRestoreState) {
-        // Only re-enable interrupts if they were enabled before the critical section.
-        if was_active {
-            // NOTE: Fence guarantees are provided by interrupt::enable(), which performs a
-            // `compiler_fence(SeqCst)`.
-            interrupt::enable()
-        }
+    unsafe fn release(restore_state: RawRestoreState) {
+        // NOTE: Fence guarantees are provided by primask::write_raw(), which performs a `compiler_fence(SeqCst)`.
+        primask::write_raw(restore_state);
     }
 }

cortex-m/src/interrupt.rs

@@ -66,18 +66,17 @@ pub fn free<F, R>(f: F) -> R
 where
     F: FnOnce() -> R,
 {
-    let primask = crate::register::primask::read();
+    // Backup previous state of PRIMASK register. We access the entire register directly as a
+    // u32 instead of using the primask::read() function to minimize the number of processor
+    // cycles during which interrupts are disabled.
+    let primask = crate::register::primask::read_raw();
 
     // disable interrupts
     disable();
 
     let r = f();
 
-    // If the interrupts were active before our `disable` call, then re-enable
-    // them. Otherwise, keep them disabled
-    if primask.is_active() {
-        unsafe { enable() }
-    }
+    crate::register::primask::write_raw(primask);
 
     r
 }

cortex-m/src/register/primask.rs

@@ -2,6 +2,8 @@
 
 #[cfg(cortex_m)]
 use core::arch::asm;
+#[cfg(cortex_m)]
+use core::sync::atomic::{compiler_fence, Ordering};
 
 /// All exceptions with configurable priority are ...
 #[derive(Clone, Copy, Debug, Eq, PartialEq)]
@@ -26,15 +28,35 @@ impl Primask {
     }
 }
 
-/// Reads the CPU register
+/// Reads the prioritizable interrupt mask
 #[cfg(cortex_m)]
 #[inline]
 pub fn read() -> Primask {
-    let r: u32;
-    unsafe { asm!("mrs {}, PRIMASK", out(reg) r, options(nomem, nostack, preserves_flags)) };
-    if r & (1 << 0) == (1 << 0) {
+    if read_raw() & (1 << 0) == (1 << 0) {
         Primask::Inactive
     } else {
         Primask::Active
     }
 }
+
+/// Reads the entire PRIMASK register
+/// Note that bits [31:1] are reserved and UNK (Unknown)
+#[cfg(cortex_m)]
+#[inline]
+pub fn read_raw() -> u32 {
+    let r: u32;
+    unsafe { asm!("mrs {}, PRIMASK", out(reg) r, options(nomem, nostack, preserves_flags)) };
+    r
+}
+
+/// Writes the entire PRIMASK register
+/// Note that bits [31:1] are reserved and SBZP (Should-Be-Zero-or-Preserved)
+#[cfg(cortex_m)]
+#[inline]
+pub fn write_raw(r: u32) {
+    // Ensure no preceeding memory accesses are reordered to after interrupts are possibly enabled.
+    compiler_fence(Ordering::SeqCst);
+    unsafe { asm!("msr PRIMASK, {}", in(reg) r, options(nomem, nostack, preserves_flags)) };
+    // Ensure no subsequent memory accesses are reordered to before interrupts are possibly disabled.
+    compiler_fence(Ordering::SeqCst);
+}

testsuite/src/main.rs

@@ -2,6 +2,7 @@
 #![no_std]
 
 extern crate cortex_m_rt;
+use core::sync::atomic::{AtomicBool, Ordering};
 
 #[cfg(target_env = "")] // appease clippy
 #[panic_handler]
@@ -11,8 +12,16 @@ fn panic(info: &core::panic::PanicInfo) -> ! {
     minitest::fail()
 }
 
+static EXCEPTION_FLAG: AtomicBool = AtomicBool::new(false);
+
+#[cortex_m_rt::exception]
+fn PendSV() {
+    EXCEPTION_FLAG.store(true, Ordering::SeqCst);
+}
+
 #[minitest::tests]
 mod tests {
+    use crate::{Ordering, EXCEPTION_FLAG};
     use minitest::log;
 
     #[init]
@@ -51,4 +60,30 @@ mod tests {
             assert!(!p.DWT.has_cycle_counter());
         }
     }
+
+    #[test]
+    fn critical_section_nesting() {
+        EXCEPTION_FLAG.store(false, Ordering::SeqCst);
+        critical_section::with(|_| {
+            critical_section::with(|_| {
+                cortex_m::peripheral::SCB::set_pendsv();
+                assert!(!EXCEPTION_FLAG.load(Ordering::SeqCst));
+            });
+            assert!(!EXCEPTION_FLAG.load(Ordering::SeqCst));
+        });
+        assert!(EXCEPTION_FLAG.load(Ordering::SeqCst));
+    }
+
+    #[test]
+    fn interrupt_free_nesting() {
+        EXCEPTION_FLAG.store(false, Ordering::SeqCst);
+        cortex_m::interrupt::free(|| {
+            cortex_m::interrupt::free(|| {
+                cortex_m::peripheral::SCB::set_pendsv();
+                assert!(!EXCEPTION_FLAG.load(Ordering::SeqCst));
+            });
+            assert!(!EXCEPTION_FLAG.load(Ordering::SeqCst));
+        });
+        assert!(EXCEPTION_FLAG.load(Ordering::SeqCst));
+    }
 }