fix get/set_priority

these methods must write to the highest bits of the IPR registers

1 files changed, 9 insertions, 2 deletions

diff --git a/src/peripheral/nvic.rs b/src/peripheral/nvic.rs
index 91e158c..054a74e 100644
--- a/src/peripheral/nvic.rs
+++ b/src/peripheral/nvic.rs
@@ -3,6 +3,12 @@
 use interrupt::Nr;
 use volatile_register::{RO, RW};
 
+#[cfg(thumbv6m)]
+const PRIORITY_BITS: u8 = 2;
+
+#[cfg(not(thumbv6m))]
+const PRIORITY_BITS: u8 = 4;
+
 /// Registers
 #[repr(C)]
 pub struct Registers {
@@ -59,7 +65,7 @@ impl Registers {
     {
         let nr = interrupt.nr();
 
-        self.ipr[usize::from(nr)].read()
+        self.ipr[usize::from(nr)].read() >> (8 - PRIORITY_BITS)
     }
 
     /// Is `interrupt` active or pre-empted and stacked
@@ -107,6 +113,7 @@ impl Registers {
     {
         let nr = interrupt.nr();
 
-        self.ipr[usize::from(nr)].write(prio);
+        self.ipr[usize::from(nr)].write((prio & ((1 << PRIORITY_BITS) - 1)) <<
+                                        (8 - PRIORITY_BITS));
     }
 }