Add helper function to enter unprivileged mode

cortex-m/src/asm.rs

@@ -176,9 +176,9 @@ pub unsafe fn semihosting_syscall(nr: u32, arg: u32) -> u32 {
     call_asm!(__sh_syscall(nr: u32, arg: u32) -> u32)
 }
 
-/// Switch to unprivileged mode.
+/// Switch to unprivileged mode using the Process Stack
 ///
-/// Sets CONTROL.SPSEL (setting the program stack to be the active
+/// Sets CONTROL.SPSEL (setting the Process Stack to be the active
 /// stack) and CONTROL.nPRIV (setting unprivileged mode), updates the
 /// program stack pointer to the address in `psp`, then jumps to the
 /// address in `entry`.
@@ -194,19 +194,71 @@ pub unsafe fn semihosting_syscall(nr: u32, arg: u32) -> u32 {
 ///   it, you may wish to set the `PSPLIM` register to guard against this.
 #[cfg(cortex_m)]
 #[inline(always)]
-pub unsafe fn enter_unprivileged(psp: *const u32, entry: fn() -> !) -> ! {
-    core::arch::asm!(
-        "mrs {tmp}, CONTROL",
-        "orr {tmp}, #3",
-        "msr PSP, {psp}",
-        "msr CONTROL, {tmp}",
-        "isb",
-        "bx {ent}",
-        tmp = in(reg) 0,
-        psp = in(reg) psp,
-        ent = in(reg) entry,
-        options(noreturn, nomem, nostack)
-    );
+pub unsafe fn enter_unprivileged_psp(psp: *const u32, entry: extern "C" fn() -> !) -> ! {
+    use crate::register::control::{Control, Npriv, Spsel};
+    const CONTROL_FLAGS: u32 = {
+        Control::from_bits(0)
+            .with_npriv(Npriv::Unprivileged)
+            .with_spsel(Spsel::Psp)
+            .bits()
+    };
+    unsafe {
+        core::arch::asm!(
+            "msr     PSP, {psp}",
+            "mrs     {tmp}, CONTROL",
+            "orrs    {tmp}, {flags}",
+            "msr     CONTROL, {tmp}",
+            "isb",
+            "bx      {ent}",
+            tmp = in(reg) 0,
+            flags = in(reg) CONTROL_FLAGS,
+            psp = in(reg) psp,
+            ent = in(reg) entry,
+            options(noreturn, nostack)
+        );
+    }
+}
+
+/// Switch to using the Process Stack, but remain in Privileged Mode
+///
+/// Sets CONTROL.SPSEL (setting the Process Stack to be the active stack) but
+/// leaves CONTROL.nPRIV alone, updates the program stack pointer to the
+/// address in `psp`, then jumps to the address in `entry`.
+///
+/// # Safety
+///
+/// * `psp` and `entry` must point to valid stack memory and executable code,
+///   respectively.
+/// * `psp` must be 8 bytes aligned and point to stack top as stack grows
+///   towards lower addresses.
+/// * The size of the stack provided here must be large enough for your
+///   program - stack overflows are obviously UB. If your processor supports
+///   it, you may wish to set the `PSPLIM` register to guard against this.
+#[cfg(cortex_m)]
+#[inline(always)]
+pub unsafe fn enter_privileged_psp(psp: *const u32, entry: extern "C" fn() -> !) -> ! {
+    use crate::register::control::{Control, Npriv, Spsel};
+    const CONTROL_FLAGS: u32 = {
+        Control::from_bits(0)
+            .with_npriv(Npriv::Privileged)
+            .with_spsel(Spsel::Psp)
+            .bits()
+    };
+    unsafe {
+        core::arch::asm!(
+            "msr     PSP, {psp}",
+            "mrs     {tmp}, CONTROL",
+            "orrs    {tmp}, {flags}",
+            "msr     CONTROL, {tmp}",
+            "isb",
+            "bx      {ent}",
+            tmp = in(reg) 0,
+            flags = in(reg) CONTROL_FLAGS,
+            psp = in(reg) psp,
+            ent = in(reg) entry,
+            options(noreturn, nostack)
+        );
+    }
 }
 
 /// Bootstrap.

cortex-m/src/lib.rs

@@ -111,6 +111,7 @@ pub mod interrupt;
 pub mod itm;
 pub mod peripheral;
 pub mod prelude;
+pub mod psp;
 pub mod register;
 
 pub use crate::peripheral::Peripherals;

cortex-m/src/psp.rs

@@ -0,0 +1,107 @@
+//! Process Stack Pointer support
+
+// This is a useful lint for functions like 'asm::wfi()' but it's not a useful
+// lint here.
+#![allow(clippy::missing_inline_in_public_items)]
+
+use core::{
+    cell::UnsafeCell,
+    sync::atomic::{AtomicBool, Ordering},
+};
+
+/// Represents access to a [`Stack`]
+pub struct StackHandle(*mut u32, usize);
+
+impl StackHandle {
+    /// Get the pointer to the top of the stack
+    pub fn top(&mut self) -> *mut u32 {
+        // SAFETY: The stack was this big when we constructed the handle
+        unsafe { self.0.add(self.1) }
+    }
+
+    /// Get the pointer to the top of the stack
+    pub fn bottom(&mut self) -> *mut u32 {
+        self.0
+    }
+}
+
+/// A stack you can use as your Process Stack (PSP)
+///
+/// The const-param N is the size **in 32-bit words**
+#[repr(align(8), C)]
+pub struct Stack<const N: usize> {
+    space: UnsafeCell<[u32; N]>,
+    taken: AtomicBool,
+}
+
+impl<const N: usize> Stack<N> {
+    /// Const-initialise a Stack
+    ///
+    /// Use a turbofish to specify the size, like:
+    ///
+    /// ```rust
+    /// # use cortex_m::psp::Stack;
+    /// static PSP_STACK: Stack::<4096> = Stack::new();
+    /// fn example() {
+    ///    let handle = PSP_STACK.take_handle();
+    ///    // ...
+    /// }
+    /// ```
+    pub const fn new() -> Stack<N> {
+        Stack {
+            space: UnsafeCell::new([0; N]),
+            taken: AtomicBool::new(false),
+        }
+    }
+
+    /// Return the top of the stack
+    pub fn take_handle(&self) -> StackHandle {
+        if self.taken.load(Ordering::Acquire) {
+            panic!("Cannot get two handles to one stack!");
+        }
+        self.taken.store(true, Ordering::Release);
+
+        let start = self.space.get() as *mut u32;
+        StackHandle(start, N)
+    }
+}
+
+unsafe impl<const N: usize> Sync for Stack<N> {}
+
+impl<const N: usize> core::default::Default for Stack<N> {
+    fn default() -> Self {
+        Stack::new()
+    }
+}
+
+/// Switch to unprivileged mode running on the Process Stack Pointer (PSP)
+///
+/// In Unprivileged Mode, code can no longer perform privileged operations,
+/// such as disabling interrupts.
+///
+#[cfg(cortex_m)]
+pub fn switch_to_unprivileged_psp(mut psp_stack: StackHandle, function: extern "C" fn() -> !) -> ! {
+    // set the stack limit
+    #[cfg(armv8m_main)]
+    unsafe {
+        crate::register::psplim::write(psp_stack.bottom() as u32);
+    }
+    // do the switch
+    unsafe {
+        crate::asm::enter_unprivileged_psp(psp_stack.top(), function);
+    }
+}
+
+/// Switch to running on the Process Stack Pointer (PSP), but remain in privileged mode
+#[cfg(cortex_m)]
+pub fn switch_to_privileged_psp(mut psp_stack: StackHandle, function: extern "C" fn() -> !) -> ! {
+    // set the stack limit
+    #[cfg(armv8m_main)]
+    unsafe {
+        crate::register::psplim::write(psp_stack.bottom() as u32);
+    }
+    // do the switch
+    unsafe {
+        crate::asm::enter_privileged_psp(psp_stack.top(), function);
+    }
+}

cortex-m/src/register/control.rs

@@ -9,13 +9,13 @@ pub struct Control {
 impl Control {
     /// Creates a `Control` value from raw bits.
     #[inline]
-    pub fn from_bits(bits: u32) -> Self {
+    pub const fn from_bits(bits: u32) -> Self {
         Self { bits }
     }
 
     /// Returns the contents of the register as raw bits
     #[inline]
-    pub fn bits(self) -> u32 {
+    pub const fn bits(self) -> u32 {
         self.bits
     }
 
@@ -39,6 +39,17 @@ impl Control {
         }
     }
 
+    /// Sets the thread mode privilege level value (nPRIV).
+    #[inline]
+    pub const fn with_npriv(self, npriv: Npriv) -> Self {
+        let mask = 1 << 0;
+        let bits = match npriv {
+            Npriv::Unprivileged => self.bits | mask,
+            Npriv::Privileged => self.bits & !mask,
+        };
+        Self { bits }
+    }
+
     /// Currently active stack pointer
     #[inline]
     pub fn spsel(self) -> Spsel {
@@ -59,6 +70,17 @@ impl Control {
         }
     }
 
+    /// Sets the SPSEL value.
+    #[inline]
+    pub const fn with_spsel(self, spsel: Spsel) -> Self {
+        let mask = 1 << 1;
+        let bits = match spsel {
+            Spsel::Psp => self.bits | mask,
+            Spsel::Msp => self.bits & !mask,
+        };
+        Self { bits }
+    }
+
     /// Whether context floating-point is currently active
     #[inline]
     pub fn fpca(self) -> Fpca {

testsuite/src/main.rs

@@ -14,6 +14,10 @@ fn panic(info: &core::panic::PanicInfo) -> ! {
 
 static EXCEPTION_FLAG: AtomicBool = AtomicBool::new(false);
 
+const STACK_SIZE_WORDS: usize = 1024;
+
+static STACK: cortex_m::psp::Stack<STACK_SIZE_WORDS> = cortex_m::psp::Stack::new();
+
 #[cortex_m_rt::exception]
 fn PendSV() {
     EXCEPTION_FLAG.store(true, Ordering::SeqCst);
@@ -86,4 +90,13 @@ mod tests {
         });
         assert!(EXCEPTION_FLAG.load(Ordering::SeqCst));
     }
+
+    #[test]
+    fn check_stack_handles() {
+        let mut handle = super::STACK.take_handle();
+        let top = handle.top();
+        let bottom = handle.bottom();
+        let delta = unsafe { top.offset_from(bottom) };
+        assert_eq!(delta as usize, super::STACK_SIZE_WORDS);
+    }
 }