From 398a5ebc5c7ad10e3278aea7ff61da2644c04748 Mon Sep 17 00:00:00 2001
From: Jorge Aparicio <japaricious@gmail.com>
Date: Thu, 13 Apr 2017 23:52:02 -0500
Subject: [PATCH] add `critical`, a global critical section

---
 build.rs   |  2 +-
 src/lib.rs | 45 +++++++++++++++++++++++++--------------------
 2 files changed, 26 insertions(+), 21 deletions(-)

diff --git a/build.rs b/build.rs
index 7f8574097c..2879705f80 100644
--- a/build.rs
+++ b/build.rs
@@ -93,7 +93,7 @@ fn main() {
     let u = Ident::new(format!("U{}", (1 << bits)));
     tokens.push(
         quote! {
-            #[doc(hidden)]
+            /// Maximum ceiling
             pub type CMAX = C<::typenum::#u>;
 
             /// Maximum priority level
diff --git a/src/lib.rs b/src/lib.rs
index e2937fee3e..4c94019a6b 100644
--- a/src/lib.rs
+++ b/src/lib.rs
@@ -22,8 +22,6 @@ pub use cortex_m::asm::wfi;
 
 #[doc(hidden)]
 pub use cortex_m::peripheral::NVIC;
-#[doc(hidden)]
-pub use cortex_m::interrupt::free;
 
 macro_rules! barrier {
     () => {
@@ -128,13 +126,7 @@ where
             let old_basepri = basepri::read();
             basepri_max::write(<CEILING>::hw());
             barrier!();
-            let ret = f(
-                &*self.data.get(),
-                C {
-                    _0: (),
-                    _marker: PhantomData,
-                },
-            );
+            let ret = f(&*self.data.get(), C { _marker: PhantomData });
             barrier!();
             basepri::write(old_basepri);
             ret
@@ -252,13 +244,7 @@ where
             let old_basepri = basepri::read();
             basepri_max::write(<CEILING>::hw());
             barrier!();
-            let ret = f(
-                &*self.peripheral.get(),
-                C {
-                    _0: (),
-                    _marker: PhantomData,
-                },
-            );
+            let ret = f(&*self.peripheral.get(), C { _marker: PhantomData });
             barrier!();
             basepri::write(old_basepri);
             ret
@@ -272,6 +258,27 @@ where
 {
 }
 
+/// A global critical section
+///
+/// No task can preempt this critical section
+pub fn critical<R, F>(f: F) -> R
+where
+    F: FnOnce(CMAX) -> R,
+{
+    let primask = ::cortex_m::register::primask::read();
+    ::cortex_m::interrupt::disable();
+
+    let r = f(C { _marker: PhantomData });
+
+    // If the interrupts were active before our `disable` call, then re-enable
+    // them. Otherwise, keep them disabled
+    if primask.is_active() {
+        ::cortex_m::interrupt::enable();
+    }
+
+    r
+}
+
 /// Requests the execution of the task `task`
 pub fn request<T, P>(_task: fn(T, P))
 where
@@ -301,13 +308,11 @@ where
 
 /// A type-level ceiling
 pub struct C<T> {
-    _0: (),
     _marker: PhantomData<T>,
 }
 
 /// A type-level priority
 pub struct P<T> {
-    _0: (),
     _marker: PhantomData<T>,
 }
 
@@ -358,8 +363,8 @@ macro_rules! tasks {
         $($task:ident: ($Interrupt:ident, $P:ident),)*
     }) => {
         fn main() {
-            $crate::free(|_| {
-                init(unsafe { ::core::ptr::read(0x0 as *const $crate::CMAX )});
+            $crate::critical(|cmax| {
+                init(cmax);
                 set_priorities();
                 enable_tasks();
             });