From 9e97102e124adbfc4bdabf0ecdd5745d216ebea6 Mon Sep 17 00:00:00 2001
From: pca006132 <john.lck40@gmail.com>
Date: Tue, 4 Aug 2020 12:59:23 +0800
Subject: [PATCH] libcortex_a9: implemented semaphore.

---
 libcortex_a9/src/lib.rs       |  1 +
 libcortex_a9/src/semaphore.rs | 71 +++++++++++++++++++++++++++++++++++
 2 files changed, 72 insertions(+)
 create mode 100644 libcortex_a9/src/semaphore.rs

diff --git a/libcortex_a9/src/lib.rs b/libcortex_a9/src/lib.rs
index bd73521..9df5900 100644
--- a/libcortex_a9/src/lib.rs
+++ b/libcortex_a9/src/lib.rs
@@ -11,6 +11,7 @@ pub mod cache;
 pub mod mmu;
 pub mod mutex;
 pub mod sync_channel;
+pub mod semaphore;
 mod uncached;
 mod fpu;
 pub use uncached::UncachedSlice;
diff --git a/libcortex_a9/src/semaphore.rs b/libcortex_a9/src/semaphore.rs
new file mode 100644
index 0000000..ee0799a
--- /dev/null
+++ b/libcortex_a9/src/semaphore.rs
@@ -0,0 +1,71 @@
+use super::asm::{sev, wfe};
+use core::{
+    task::{Context, Poll},
+    pin::Pin,
+    future::Future
+};
+use core::sync::atomic::{AtomicI32, Ordering};
+
+pub struct Semaphore {
+    value: AtomicI32,
+    max: i32
+}
+
+impl Semaphore {
+    pub fn new(value: i32, max: i32) -> Self {
+        Semaphore { value: AtomicI32::new(value), max}
+    }
+
+    pub fn try_wait(&self) -> Option<()> {
+        loop {
+            let value = self.value.load(Ordering::Relaxed);
+            if value > 0 {
+                if self.value.compare_and_swap(value, value - 1, Ordering::SeqCst) == value {
+                    return Some(());
+                }
+            } else {
+                return None;
+            }
+        }
+    }
+
+    pub fn wait(&self) {
+        while self.try_wait().is_none() {
+            wfe();
+        }
+    }
+
+    pub async fn async_wait(&self) {
+        struct Fut<'a>(&'a Semaphore);
+
+        impl Future for Fut<'_> {
+            type Output = ();
+            fn poll(self: Pin<&mut Self>, cx: &mut Context<'_>) -> Poll<Self::Output> {
+                match self.0.try_wait() {
+                    Some(_) => Poll::Ready(()),
+                    None => {
+                        cx.waker().wake_by_ref();
+                        Poll::Pending
+                    }
+                }
+            }
+        }
+
+        Fut(&self).await
+    }
+
+    pub fn signal(&self) {
+        loop {
+            let value = self.value.load(Ordering::Relaxed);
+            if value < self.max {
+                if self.value.compare_and_swap(value, value + 1, Ordering::SeqCst) == value {
+                    sev();
+                    return;
+                }
+            } else {
+                return;
+            }
+        }
+    }
+}
+