From fb53bc289ddfb82efc98eeae9e3603e65c1fe68d Mon Sep 17 00:00:00 2001
From: Kevin Bube <k.bube@web.de>
Date: Fri, 25 Apr 2025 18:32:47 +0200
Subject: [PATCH] Add enter_unprivileged() function

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

Fixes #583
---
 cortex-m/src/asm.rs | 29 +++++++++++++++++++++++++++++
 1 file changed, 29 insertions(+)

diff --git a/cortex-m/src/asm.rs b/cortex-m/src/asm.rs
index 47b286a4..419e77c1 100644
--- a/cortex-m/src/asm.rs
+++ b/cortex-m/src/asm.rs
@@ -244,6 +244,35 @@ 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. `psp` must be 8 bytes aligned and point to
+/// stack top as stack grows towards lower addresses.
+#[cfg(cortex_m)]
+#[inline(always)]
+pub unsafe fn enter_unprivileged(psp: *const u32, entry: fn() -> !) -> ! {
+    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)
+    );
+}
+
 /// Bootstrap.
 ///
 /// Clears CONTROL.SPSEL (setting the main stack to be the active stack),