Add setting of priority to interrupts

1 files changed, 36 insertions, 0 deletions

diff --git a/rtic-monotonics/src/lib.rs b/rtic-monotonics/src/lib.rs
index fab0b2d..04ce4e2 100644
--- a/rtic-monotonics/src/lib.rs
+++ b/rtic-monotonics/src/lib.rs
@@ -26,6 +26,42 @@ pub mod rp2040;
 ))]
 pub mod nrf;
 
+#[allow(dead_code)]
+pub(crate) const fn cortex_logical2hw(logical: u8, nvic_prio_bits: u8) -> u8 {
+    ((1 << nvic_prio_bits) - logical) << (8 - nvic_prio_bits)
+}
+
+#[cfg(any(
+    feature = "rp2040",
+    feature = "nrf52810",
+    feature = "nrf52811",
+    feature = "nrf52832",
+    feature = "nrf52833",
+    feature = "nrf52840",
+    feature = "nrf5340-app",
+    feature = "nrf5340-net",
+    feature = "nrf9160",
+))]
+pub(crate) unsafe fn set_monotonic_prio(
+    prio_bits: u8,
+    interrupt: impl cortex_m::interrupt::InterruptNumber,
+) {
+    extern "C" {
+        static RTIC_ASYNC_MAX_LOGICAL_PRIO: u8;
+    }
+
+    let max_prio = RTIC_ASYNC_MAX_LOGICAL_PRIO.max(1).min(1 << prio_bits);
+
+    let hw_prio = crate::cortex_logical2hw(max_prio, prio_bits);
+
+    // We take ownership of the entire IRQ and all settings to it, we only change settings
+    // for the IRQ we control.
+    // This will also compile-error in case the NVIC changes in size.
+    let mut nvic: cortex_m::peripheral::NVIC = core::mem::transmute(());
+
+    nvic.set_priority(interrupt, hw_prio);
+}
+
 /// This marker is implemented on an interrupt token to enforce that the right tokens
 /// are given to the correct monotonic implementation.
 ///