Allow writing to the CONTROL register

9 files changed, 74 insertions, 5 deletions

diff --git a/asm.s b/asm.s
index 1ad6fed..fd2c7fa 100644
--- a/asm.s
+++ b/asm.s
@@ -5,13 +5,21 @@ __bkpt:
   bkpt
   bx lr
 
-  .section .text.__control
-  .global __control
+  .section .text.__control_r
+  .global __control_r
   .thumb_func
-__control:
+__control_r:
   mrs r0, CONTROL
   bx lr
 
+  .section .text.__control_w
+  .global __control_w
+  .thumb_func
+__control_w:
+  msr CONTROL, r0
+  bx lr
+
+
   .section .text.__cpsid
   .global __cpsid
   .thumb_func
diff --git a/bin/thumbv6m-none-eabi.a b/bin/thumbv6m-none-eabi.a
index 2b04c3a..0684d4e 100644
--- a/bin/thumbv6m-none-eabi.a
+++ b/bin/thumbv6m-none-eabi.a
diff --git a/bin/thumbv7em-none-eabi.a b/bin/thumbv7em-none-eabi.a
index 51f1b0e..cbfe5ae 100644
--- a/bin/thumbv7em-none-eabi.a
+++ b/bin/thumbv7em-none-eabi.a
diff --git a/bin/thumbv7em-none-eabihf.a b/bin/thumbv7em-none-eabihf.a
index 51f1b0e..cbfe5ae 100644
--- a/bin/thumbv7em-none-eabihf.a
+++ b/bin/thumbv7em-none-eabihf.a
diff --git a/bin/thumbv7m-none-eabi.a b/bin/thumbv7m-none-eabi.a
index 15c638d..6e77aeb 100644
--- a/bin/thumbv7m-none-eabi.a
+++ b/bin/thumbv7m-none-eabi.a
diff --git a/bin/thumbv8m.base-none-eabi.a b/bin/thumbv8m.base-none-eabi.a
index 7a88f95..78ae15c 100644
--- a/bin/thumbv8m.base-none-eabi.a
+++ b/bin/thumbv8m.base-none-eabi.a
diff --git a/bin/thumbv8m.main-none-eabi.a b/bin/thumbv8m.main-none-eabi.a
index a0b35de..e751232 100644
--- a/bin/thumbv8m.main-none-eabi.a
+++ b/bin/thumbv8m.main-none-eabi.a
diff --git a/bin/thumbv8m.main-none-eabihf.a b/bin/thumbv8m.main-none-eabihf.a
index a0b35de..e751232 100644
--- a/bin/thumbv8m.main-none-eabihf.a
+++ b/bin/thumbv8m.main-none-eabihf.a
diff --git a/src/register/control.rs b/src/register/control.rs
index b6b6676..c08522c 100644
--- a/src/register/control.rs
+++ b/src/register/control.rs
@@ -7,6 +7,11 @@ pub struct Control {
 }
 
 impl Control {
+    /// Creates a `Control` value from raw bits.
+    pub fn from_bits(bits: u32) -> Self {
+        Self { bits }
+    }
+
     /// Returns the contents of the register as raw bits
     pub fn bits(&self) -> u32 {
         self.bits
@@ -21,6 +26,15 @@ impl Control {
         }
     }
 
+    /// Sets the thread mode privilege level value (nPRIV).
+    pub fn set_npriv(&mut self, npriv: Npriv) {
+        let mask = 1 << 0;
+        match npriv {
+            Npriv::Unprivileged => self.bits |= mask,
+            Npriv::Privileged => self.bits &= !mask,
+        }
+    }
+
     /// Currently active stack pointer
     pub fn spsel(&self) -> Spsel {
         if self.bits & (1 << 1) == (1 << 1) {
@@ -30,6 +44,15 @@ impl Control {
         }
     }
 
+    /// Sets the SPSEL value.
+    pub fn set_spsel(&mut self, spsel: Spsel) {
+        let mask = 1 << 1;
+        match spsel {
+            Spsel::Psp => self.bits |= mask,
+            Spsel::Msp => self.bits &= !mask,
+        }
+    }
+
     /// Whether context floating-point is currently active
     pub fn fpca(&self) -> Fpca {
         if self.bits & (1 << 2) == (1 << 2) {
@@ -38,6 +61,15 @@ impl Control {
             Fpca::NotActive
         }
     }
+
+    /// Sets the FPCA value.
+    pub fn set_fpca(&mut self, fpca: Fpca) {
+        let mask = 1 << 2;
+        match fpca {
+            Fpca::Active => self.bits |= mask,
+            Fpca::NotActive => self.bits &= !mask,
+        }
+    }
 }
 
 /// Thread mode privilege level
@@ -120,10 +152,10 @@ pub fn read() -> Control {
                 #[cfg(not(feature = "inline-asm"))]
                 () => unsafe {
                     extern "C" {
-                        fn __control() -> u32;
+                        fn __control_r() -> u32;
                     }
 
-                    __control()
+                    __control_r()
                 },
             };
 
@@ -134,3 +166,32 @@ pub fn read() -> Control {
         () => unimplemented!(),
     }
 }
+
+/// Writes to the CPU register.
+#[inline]
+pub unsafe fn write(_control: Control) {
+    match () {
+        #[cfg(cortex_m)]
+        () => {
+            let r = match () {
+                #[cfg(feature = "inline-asm")]
+                () => {
+                    let control = _control.bits();
+                    unsafe { asm!("msr CONTROL, $0" :: "r"(control) : "memory" : "volatile") }
+                }
+
+                #[cfg(not(feature = "inline-asm"))]
+                () => unsafe {
+                    extern "C" {
+                        fn __control_w() -> u32;
+                    }
+
+                    __control_w(_control.bits())
+                },
+            };
+        }
+
+        #[cfg(not(cortex_m))]
+        () => unimplemented!(),
+    }
+}