From 12669124a42d763c9d3fb31ef46a48a00bec6443 Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Mon, 3 Aug 2020 11:24:18 +0800
Subject: [PATCH] libcortex_a9/mutex: added interrupt critical section mask.

---
 libcortex_a9/src/asm.rs   | 26 ++++++++++++++++++++++++++
 libcortex_a9/src/mutex.rs | 15 ++++++++++++---
 2 files changed, 38 insertions(+), 3 deletions(-)

diff --git a/libcortex_a9/src/asm.rs b/libcortex_a9/src/asm.rs
index 7c365a9..e513275 100644
--- a/libcortex_a9/src/asm.rs
+++ b/libcortex_a9/src/asm.rs
@@ -40,6 +40,32 @@ pub unsafe fn enable_irq() {
     llvm_asm!("cpsie i":::: "volatile");
 }
 
+/// Disable IRQ, return if IRQ was originally enabled.
+#[inline]
+pub unsafe fn enter_critical() -> bool {
+    let mut cpsr: u32;
+    llvm_asm!(
+        "mrs $0, cpsr
+         cpsid i"
+         : "=r"(cpsr) ::: "volatile");
+    (cpsr & (1 << 7)) == 0
+}
+
+#[inline]
+pub unsafe fn exit_critical(enable: bool) {
+    // https://stackoverflow.com/questions/40019929/temporarily-disable-interrupts-on-arm
+    let mask: u32 = if enable {
+        1 << 7
+    } else {
+        0
+    };
+    llvm_asm!(
+        "mrs r1, cpsr
+         bic r1, r1, $0
+         msr cpsr_c, r1"
+         :: "r"(mask) : "r1");
+}
+
 /// Exiting IRQ
 #[inline]
 pub unsafe fn exit_irq() {
diff --git a/libcortex_a9/src/mutex.rs b/libcortex_a9/src/mutex.rs
index 8d84105..4eb8628 100644
--- a/libcortex_a9/src/mutex.rs
+++ b/libcortex_a9/src/mutex.rs
@@ -41,19 +41,26 @@ impl<T> Mutex<T> {
 
     /// Lock the Mutex, blocks when already locked
     pub fn lock(&self) -> MutexGuard<T> {
+        let mut irq = unsafe { enter_critical() };
         while self.locked.compare_and_swap(UNLOCKED, LOCKED, Ordering::Acquire) != UNLOCKED {
-            wait_for_update();
+            unsafe {
+                exit_critical(irq);
+                wait_for_update();
+                irq = enter_critical();
+            }
         }
         dmb();
-        MutexGuard { mutex: self }
+        MutexGuard { mutex: self, irq }
     }
 
     pub fn try_lock(&self) -> Option<MutexGuard<T>> {
+        let irq = unsafe { enter_critical() };
         if self.locked.compare_and_swap(UNLOCKED, LOCKED, Ordering::Acquire) != UNLOCKED {
+            unsafe { exit_critical(irq) };
             None
         } else {
             dmb();
-            Some(MutexGuard { mutex: self })
+            Some(MutexGuard { mutex: self, irq })
         }
     }
 
@@ -69,6 +76,7 @@ impl<T> Mutex<T> {
 /// `Deref`/`DerefMutx`
 pub struct MutexGuard<'a, T> {
     mutex: &'a Mutex<T>,
+    irq: bool,
 }
 
 impl<'a, T> Deref for MutexGuard<'a, T> {
@@ -88,5 +96,6 @@ impl<'a, T> DerefMut for MutexGuard<'a, T> {
 impl<'a, T> Drop for MutexGuard<'a, T> {
     fn drop(&mut self) {
         self.mutex.unlock();
+        unsafe { exit_critical(self.irq) };
     }
 }