1 files changed, 84 insertions, 5 deletions

diff --git a/src/export.rs b/src/export.rs
index cb63e0ce..200c69d9 100644
--- a/src/export.rs
+++ b/src/export.rs
@@ -1,5 +1,8 @@
-/// IMPLEMENTATION DETAILS. DO NOT USE ANYTHING IN THIS MODULE
-use core::{hint, ptr};
+//! IMPLEMENTATION DETAILS. DO NOT USE ANYTHING IN THIS MODULE
+
+#[cfg(not(debug_assertions))]
+use core::hint;
+use core::{cell::Cell, ptr, u8};
 
 #[cfg(armv7m)]
 use cortex_m::register::basepri;
@@ -52,7 +55,13 @@ impl<T> MaybeUninit<T> {
         if let Some(x) = self.value.as_ref() {
             x
         } else {
-            hint::unreachable_unchecked()
+            match () {
+                // Try to catch UB when compiling in release with debug assertions enabled
+                #[cfg(debug_assertions)]
+                () => unreachable!(),
+                #[cfg(not(debug_assertions))]
+                () => hint::unreachable_unchecked(),
+            }
         }
     }
 
@@ -60,12 +69,19 @@ impl<T> MaybeUninit<T> {
         if let Some(x) = self.value.as_mut() {
             x
         } else {
-            hint::unreachable_unchecked()
+            match () {
+                // Try to catch UB when compiling in release with debug assertions enabled
+                #[cfg(debug_assertions)]
+                () => unreachable!(),
+                #[cfg(not(debug_assertions))]
+                () => hint::unreachable_unchecked(),
+            }
         }
     }
 
     pub fn set(&mut self, val: T) {
-        unsafe { ptr::write(&mut self.value, Some(val)) }
+        // NOTE(volatile) we have observed UB when this uses a plain `ptr::write`
+        unsafe { ptr::write_volatile(&mut self.value, Some(val)) }
     }
 }
 
@@ -82,3 +98,66 @@ where
     T: Sync,
 {
 }
+
+#[cfg(armv7m)]
+#[inline(always)]
+pub unsafe fn claim<T, R, F>(
+    ptr: *mut T,
+    priority: &Cell<u8>,
+    ceiling: u8,
+    nvic_prio_bits: u8,
+    f: F,
+) -> R
+where
+    F: FnOnce(&mut T) -> R,
+{
+    let current = priority.get();
+
+    if priority.get() < ceiling {
+        if ceiling == (1 << nvic_prio_bits) {
+            priority.set(u8::MAX);
+            let r = interrupt::free(|_| f(&mut *ptr));
+            priority.set(current);
+            r
+        } else {
+            priority.set(ceiling);
+            basepri::write(logical2hw(ceiling, nvic_prio_bits));
+            let r = f(&mut *ptr);
+            basepri::write(logical2hw(current, nvic_prio_bits));
+            priority.set(current);
+            r
+        }
+    } else {
+        f(&mut *ptr)
+    }
+}
+
+#[cfg(not(armv7m))]
+#[inline(always)]
+pub unsafe fn claim<T, R, F>(
+    ptr: *mut T,
+    priority: &Cell<u8>,
+    ceiling: u8,
+    _nvic_prio_bits: u8,
+    f: F,
+) -> R
+where
+    F: FnOnce(&mut T) -> R,
+{
+    let current = priority.get();
+
+    if priority.get() < ceiling {
+        priority.set(u8::MAX);
+        let r = interrupt::free(|_| f(&mut *ptr));
+        priority.set(current);
+        r
+    } else {
+        f(&mut *ptr)
+    }
+}
+
+#[cfg(armv7m)]
+#[inline]
+fn logical2hw(logical: u8, nvic_prio_bits: u8) -> u8 {
+    ((1 << nvic_prio_bits) - logical) << (8 - nvic_prio_bits)
+}