Use const control register code instead of magic numbers.

Also fixes the enter_privilged_function, which forgot to the change the
mode so we didn't actually start using the PSP.

Author: thejpster

cortex-m/src/asm.rs

@@ -195,6 +195,13 @@ pub unsafe fn semihosting_syscall(nr: u32, arg: u32) -> u32 {
 #[cfg(cortex_m)]
 #[inline(always)]
 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}",
@@ -204,7 +211,7 @@ pub unsafe fn enter_unprivileged_psp(psp: *const u32, entry: extern "C" fn() ->
             "isb",
             "bx      {ent}",
             tmp = in(reg) 0,
-            flags = in(reg) 3,
+            flags = in(reg) CONTROL_FLAGS,
             psp = in(reg) psp,
             ent = in(reg) entry,
             options(noreturn, nostack)
@@ -230,11 +237,23 @@ pub unsafe fn enter_unprivileged_psp(psp: *const u32, entry: extern "C" fn() ->
 #[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}",
+            "msr     PSP, {psp}",
+            "mrs     {tmp}, CONTROL",
+            "orrs    {tmp}, {flags}",
+            "msr     CONTROL, {tmp}",
             "isb",
-            "bx {ent}",
+            "bx      {ent}",
+            tmp = in(reg) 0,
+            flags = in(reg) CONTROL_FLAGS,
             psp = in(reg) psp,
             ent = in(reg) entry,
             options(noreturn, nostack)

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 {