diff --git a/asm.s b/asm.s index 1ad6fed7..fd2c7fa8 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 2b04c3af..0684d4e8 100644 Binary files a/bin/thumbv6m-none-eabi.a and b/bin/thumbv6m-none-eabi.a differ diff --git a/bin/thumbv7em-none-eabi.a b/bin/thumbv7em-none-eabi.a index 51f1b0e1..cbfe5ae9 100644 Binary files a/bin/thumbv7em-none-eabi.a and b/bin/thumbv7em-none-eabi.a differ diff --git a/bin/thumbv7em-none-eabihf.a b/bin/thumbv7em-none-eabihf.a index 51f1b0e1..cbfe5ae9 100644 Binary files a/bin/thumbv7em-none-eabihf.a and b/bin/thumbv7em-none-eabihf.a differ diff --git a/bin/thumbv7m-none-eabi.a b/bin/thumbv7m-none-eabi.a index 15c638d5..6e77aebe 100644 Binary files a/bin/thumbv7m-none-eabi.a and b/bin/thumbv7m-none-eabi.a differ diff --git a/bin/thumbv8m.base-none-eabi.a b/bin/thumbv8m.base-none-eabi.a index 7a88f959..78ae15cf 100644 Binary files a/bin/thumbv8m.base-none-eabi.a and b/bin/thumbv8m.base-none-eabi.a differ diff --git a/bin/thumbv8m.main-none-eabi.a b/bin/thumbv8m.main-none-eabi.a index a0b35def..e7512328 100644 Binary files a/bin/thumbv8m.main-none-eabi.a and b/bin/thumbv8m.main-none-eabi.a differ diff --git a/bin/thumbv8m.main-none-eabihf.a b/bin/thumbv8m.main-none-eabihf.a index a0b35def..e7512328 100644 Binary files a/bin/thumbv8m.main-none-eabihf.a and b/bin/thumbv8m.main-none-eabihf.a differ diff --git a/src/register/control.rs b/src/register/control.rs index b6b66766..20a48d74 100644 --- a/src/register/control.rs +++ b/src/register/control.rs @@ -7,12 +7,20 @@ pub struct Control { } impl Control { + /// Creates a `Control` value from raw bits. + #[inline] + pub fn from_bits(bits: u32) -> Self { + Self { bits } + } + /// Returns the contents of the register as raw bits + #[inline] pub fn bits(&self) -> u32 { self.bits } /// Thread mode privilege level + #[inline] pub fn npriv(&self) -> Npriv { if self.bits & (1 << 0) == (1 << 0) { Npriv::Unprivileged @@ -21,7 +29,18 @@ impl Control { } } + /// Sets the thread mode privilege level value (nPRIV). + #[inline] + 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 + #[inline] pub fn spsel(&self) -> Spsel { if self.bits & (1 << 1) == (1 << 1) { Spsel::Psp @@ -30,7 +49,18 @@ impl Control { } } + /// Sets the SPSEL value. + #[inline] + 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 + #[inline] pub fn fpca(&self) -> Fpca { if self.bits & (1 << 2) == (1 << 2) { Fpca::Active @@ -38,6 +68,16 @@ impl Control { Fpca::NotActive } } + + /// Sets the FPCA value. + #[inline] + 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 @@ -51,11 +91,13 @@ pub enum Npriv { impl Npriv { /// Is in privileged thread mode? + #[inline] pub fn is_privileged(&self) -> bool { *self == Npriv::Privileged } /// Is in unprivileged thread mode? + #[inline] pub fn is_unprivileged(&self) -> bool { *self == Npriv::Unprivileged } @@ -72,11 +114,13 @@ pub enum Spsel { impl Spsel { /// Is MSP the current stack pointer? + #[inline] pub fn is_msp(&self) -> bool { *self == Spsel::Msp } /// Is PSP the current stack pointer? + #[inline] pub fn is_psp(&self) -> bool { *self == Spsel::Psp } @@ -93,11 +137,13 @@ pub enum Fpca { impl Fpca { /// Is a floating-point context active? + #[inline] pub fn is_active(&self) -> bool { *self == Fpca::Active } /// Is a floating-point context not active? + #[inline] pub fn is_not_active(&self) -> bool { *self == Fpca::NotActive } @@ -120,10 +166,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 +180,30 @@ pub fn read() -> Control { () => unimplemented!(), } } + +/// Writes to the CPU register. +#[inline] +pub unsafe fn write(_control: Control) { + match () { + #[cfg(cortex_m)] + () => match () { + #[cfg(feature = "inline-asm")] + () => { + let control = _control.bits(); + asm!("msr CONTROL, $0" :: "r"(control) : "memory" : "volatile"); + } + + #[cfg(not(feature = "inline-asm"))] + () => { + extern "C" { + fn __control_w(bits: u32); + } + + __control_w(_control.bits()); + } + }, + + #[cfg(not(cortex_m))] + () => unimplemented!(), + } +}