From 03301f24ab3f7fefafff192b3c468f6af1213e7b Mon Sep 17 00:00:00 2001
From: joboet <jonasboettiger@icloud.com>
Date: Tue, 17 Oct 2023 10:40:24 +0200
Subject: [PATCH] std: implement thread parking for xous

---
 library/std/src/lib.rs                     |  1 +
 library/std/src/sys/xous/mod.rs            |  1 -
 library/std/src/sys/xous/thread_parking.rs | 88 ++++++++++++++++++++++
 3 files changed, 89 insertions(+), 1 deletion(-)
 create mode 100644 library/std/src/sys/xous/thread_parking.rs

diff --git a/library/std/src/lib.rs b/library/std/src/lib.rs
index aaf20875129..fbb2d846e00 100644
--- a/library/std/src/lib.rs
+++ b/library/std/src/lib.rs
@@ -336,6 +336,7 @@
 #![feature(portable_simd)]
 #![feature(prelude_2024)]
 #![feature(ptr_as_uninit)]
+#![feature(ptr_from_ref)]
 #![feature(raw_os_nonzero)]
 #![feature(round_ties_even)]
 #![feature(slice_internals)]
diff --git a/library/std/src/sys/xous/mod.rs b/library/std/src/sys/xous/mod.rs
index 6d5c218d195..c2550dcfd83 100644
--- a/library/std/src/sys/xous/mod.rs
+++ b/library/std/src/sys/xous/mod.rs
@@ -28,7 +28,6 @@ pub mod process;
 pub mod stdio;
 pub mod thread;
 pub mod thread_local_key;
-#[path = "../unsupported/thread_parking.rs"]
 pub mod thread_parking;
 pub mod time;
 
diff --git a/library/std/src/sys/xous/thread_parking.rs b/library/std/src/sys/xous/thread_parking.rs
new file mode 100644
index 00000000000..3e77ecf0c73
--- /dev/null
+++ b/library/std/src/sys/xous/thread_parking.rs
@@ -0,0 +1,88 @@
+use crate::os::xous::ffi::blocking_scalar;
+use crate::os::xous::services::{ticktimer_server, TicktimerScalar};
+use crate::pin::Pin;
+use crate::ptr;
+use crate::sync::atomic::{
+    AtomicI8,
+    Ordering::{Acquire, Release},
+};
+use crate::time::Duration;
+
+const NOTIFIED: i8 = 1;
+const EMPTY: i8 = 0;
+const PARKED: i8 = -1;
+
+pub struct Parker {
+    state: AtomicI8,
+}
+
+impl Parker {
+    pub unsafe fn new_in_place(parker: *mut Parker) {
+        unsafe { parker.write(Parker { state: AtomicI8::new(EMPTY) }) }
+    }
+
+    fn index(&self) -> usize {
+        ptr::from_ref(self).addr()
+    }
+
+    pub unsafe fn park(self: Pin<&Self>) {
+        // Change NOTIFIED to EMPTY and EMPTY to PARKED.
+        let state = self.state.fetch_sub(1, Acquire);
+        if state == NOTIFIED {
+            return;
+        }
+
+        // The state was set to PARKED. Wait until the `unpark` wakes us up.
+        blocking_scalar(
+            ticktimer_server(),
+            TicktimerScalar::WaitForCondition(self.index(), 0).into(),
+        )
+        .expect("failed to send WaitForCondition command");
+
+        self.state.swap(EMPTY, Acquire);
+    }
+
+    pub unsafe fn park_timeout(self: Pin<&Self>, timeout: Duration) {
+        // Change NOTIFIED to EMPTY and EMPTY to PARKED.
+        let state = self.state.fetch_sub(1, Acquire);
+        if state == NOTIFIED {
+            return;
+        }
+
+        // A value of zero indicates an indefinite wait. Clamp the number of
+        // milliseconds to the allowed range.
+        let millis = usize::max(timeout.as_millis().try_into().unwrap_or(usize::MAX), 1);
+
+        let was_timeout = blocking_scalar(
+            ticktimer_server(),
+            TicktimerScalar::WaitForCondition(self.index(), millis).into(),
+        )
+        .expect("failed to send WaitForCondition command")[0]
+            != 0;
+
+        let state = self.state.swap(EMPTY, Acquire);
+        if was_timeout && state == NOTIFIED {
+            // The state was set to NOTIFIED after we returned from the wait
+            // but before we reset the state. Therefore, a wakeup is on its
+            // way, which we need to consume here.
+            // NOTICE: this is a priority hole.
+            blocking_scalar(
+                ticktimer_server(),
+                TicktimerScalar::WaitForCondition(self.index(), 0).into(),
+            )
+            .expect("failed to send WaitForCondition command");
+        }
+    }
+
+    pub fn unpark(self: Pin<&Self>) {
+        let state = self.state.swap(NOTIFIED, Release);
+        if state == PARKED {
+            // The thread is parked, wake it up.
+            blocking_scalar(
+                ticktimer_server(),
+                TicktimerScalar::NotifyCondition(self.index(), 1).into(),
+            )
+            .expect("failed to send NotifyCondition command");
+        }
+    }
+}