diff --git a/src/lib.rs b/src/lib.rs
index abc2557aa69c59028f9e3c81991c206568f3d753..fa95775f455830e155b610f62149eade974bec81 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -137,7 +137,7 @@ where
                 if ceiling == max_priority {
                     atomic(t, |t| f(data, t))
                 } else {
-                    let old = basepri::read();
+                    //let old = basepri::read();
                     let hw = (max_priority - ceiling) << (8 - _nvic_prio_bits);
                     basepri::write(hw);
                     if cfg!(feature = "wcet_bkpt") { bkpt(); } 
@@ -147,6 +147,7 @@ where
 
                     if cfg!(feature = "wcet_bkpt") { bkpt(); } 
                     if cfg!(feature = "wcet_nop") { nop(); }
+                    let old = (max_priority - t.value()) << (8 - _nvic_prio_bits);
                     basepri::write(old);
                     ret
                 }