From 4e4ff512d9ee77179a056e7da71eda20ade4ef71 Mon Sep 17 00:00:00 2001
From: Astro <astro@spaceboyz.net>
Date: Mon, 18 Nov 2019 02:13:54 +0100
Subject: [PATCH] add cortex_a9::mutex

---
 src/cortex_a9/mod.rs   |  1 +
 src/cortex_a9/mutex.rs | 73 ++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 74 insertions(+)
 create mode 100644 src/cortex_a9/mutex.rs

diff --git a/src/cortex_a9/mod.rs b/src/cortex_a9/mod.rs
index 8d9cdb0..f7d839f 100644
--- a/src/cortex_a9/mod.rs
+++ b/src/cortex_a9/mod.rs
@@ -2,5 +2,6 @@ pub mod asm;
 pub mod regs;
 pub mod cache;
 pub mod mmu;
+pub mod mutex;
 
 global_asm!(include_str!("exceptions.s"));
diff --git a/src/cortex_a9/mutex.rs b/src/cortex_a9/mutex.rs
new file mode 100644
index 0000000..461a3d2
--- /dev/null
+++ b/src/cortex_a9/mutex.rs
@@ -0,0 +1,73 @@
+use core::ops::{Deref, DerefMut};
+use core::sync::atomic::{AtomicBool, Ordering};
+use core::cell::UnsafeCell;
+use super::asm::*;
+
+#[inline]
+fn wait_for_update() {
+    wfe();
+}
+
+#[inline]
+fn signal_update() {
+    dsb();
+    sev();
+}
+
+pub struct Mutex<T> {
+    locked: AtomicBool,
+    inner: UnsafeCell<T>,
+}
+
+unsafe impl<T: Send> Sync for Mutex<T> {}
+unsafe impl<T: Send> Send for Mutex<T> {}
+
+impl<T> Mutex<T> {
+    pub const fn new(inner: T) -> Self {
+        Mutex{
+            locked: AtomicBool::new(false),
+            inner: UnsafeCell::new(inner),
+        }
+    }
+    
+    pub fn lock(&self) -> MutexGuard<T> {
+        while self.locked.compare_and_swap(false, true, Ordering::Acquire) {
+            while self.locked.load(Ordering::Relaxed) {
+                wait_for_update();
+            }
+        }
+
+        dmb();
+
+        MutexGuard { mutex: self }
+    }
+
+    fn unlock(&self) {
+        dmb();
+        self.locked.store(false, Ordering::Release);
+        signal_update();
+    }
+}
+
+pub struct MutexGuard<'a, T> {
+    mutex: &'a Mutex<T>,
+}
+
+impl<'a, T> Deref for MutexGuard<'a, T> {
+    type Target = T;
+    fn deref(&self) -> &T {
+        unsafe { &*self.mutex.inner.get() }
+    }
+}
+
+impl<'a, T> DerefMut for MutexGuard<'a, T> {
+    fn deref_mut(&mut self) -> &mut T {
+        unsafe { &mut *self.mutex.inner.get() }
+    }
+}
+
+impl<'a, T> Drop for MutexGuard<'a, T> {
+    fn drop(&mut self) {
+        self.mutex.unlock();
+    }
+}