Add enter_unprivileged() function

This adds a function to switch to program stack and unprivileged mode.

Fixes #583

Author: kevinbube

cortex-m/src/asm.rs

@@ -244,6 +244,36 @@ pub unsafe fn semihosting_syscall(mut nr: u32, arg: u32) -> u32 {
     nr
 }
 
+/// Switch to unprivileged mode.
+///
+/// Sets CONTROL.SPSEL (setting the program 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`.
+///
+/// # Safety
+///
+/// `psp` and `entry` must point to valid stack memory and executable
+/// code, respectively.
+#[cfg(cortex_m)]
+#[inline(always)]
+fn enter_unprivileged(psp: &u32, entry: fn() -> !) -> ! {
+    unsafe {
+        asm!(
+            "mrs {tmp}, CONTROL",
+            "orr {tmp}, #2",
+            "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)
+        );
+    }
+}
+
 /// Bootstrap.
 ///
 /// Clears CONTROL.SPSEL (setting the main stack to be the active stack),