From 7059f980b887913596293b095e370408ab8d0cbb Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sat, 30 Dec 2023 08:33:21 +1100
Subject: [PATCH] HAL_SITL: implement BinarySemaphore

---
 libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h |  1 +
 libraries/AP_HAL_SITL/Semaphores.cpp          | 73 +++++++++++++++++++
 libraries/AP_HAL_SITL/Semaphores.h            | 18 +++++
 3 files changed, 92 insertions(+)

diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
index 5a360cbf59..f5924ebd8d 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
@@ -15,6 +15,7 @@ class ADCSource;
 class RCInput;
 class Util;
 class Semaphore;
+class BinarySemaphore;
 class GPIO;
 class DigitalSource;
 class DSP;
diff --git a/libraries/AP_HAL_SITL/Semaphores.cpp b/libraries/AP_HAL_SITL/Semaphores.cpp
index 216c1d98ee..0067fd9c14 100644
--- a/libraries/AP_HAL_SITL/Semaphores.cpp
+++ b/libraries/AP_HAL_SITL/Semaphores.cpp
@@ -76,4 +76,77 @@ bool Semaphore::take_nonblocking()
     return false;
 }
 
+
+/*
+  binary semaphore using pthread condition variables
+ */
+
+BinarySemaphore::BinarySemaphore(bool initial_state) :
+    AP_HAL::BinarySemaphore(initial_state)
+{
+    pthread_cond_init(&cond, NULL);
+    pending = initial_state;
+}
+
+bool BinarySemaphore::wait(uint32_t timeout_us)
+{
+    WITH_SEMAPHORE(mtx);
+    if (!pending) {
+        if (hal.scheduler->in_main_thread() ||
+            Scheduler::from(hal.scheduler)->semaphore_wait_hack_required()) {
+            /*
+              when in the main thread we need to do a busy wait to ensure
+              the clock advances
+            */
+            uint64_t end_us = AP_HAL::micros64() + timeout_us;
+            struct timespec ts {};
+            do {
+                if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) == 0) {
+                    pending = false;
+                    return true;
+                }
+                hal.scheduler->delay_microseconds(10);
+            } while (AP_HAL::micros64() < end_us);
+            return false;
+        }
+
+        struct timespec ts;
+        if (clock_gettime(CLOCK_REALTIME, &ts) != 0) {
+            return false;
+        }
+        ts.tv_sec += timeout_us/1000000UL;
+        ts.tv_nsec += (timeout_us % 1000000U) * 1000UL;
+        if (ts.tv_nsec >= 1000000000L) {
+            ts.tv_sec++;
+            ts.tv_nsec -= 1000000000L;
+        }
+        if (pthread_cond_timedwait(&cond, &mtx._lock, &ts) != 0) {
+            return false;
+        }
+    }
+    pending = false;
+    return true;
+}
+
+bool BinarySemaphore::wait_blocking(void)
+{
+    WITH_SEMAPHORE(mtx);
+    if (!pending) {
+        if (pthread_cond_wait(&cond, &mtx._lock) != 0) {
+            return false;
+        }
+    }
+    pending = false;
+    return true;
+}
+
+void BinarySemaphore::signal(void)
+{
+    WITH_SEMAPHORE(mtx);
+    if (!pending) {
+        pending = true;
+        pthread_cond_signal(&cond);
+    }
+}
+
 #endif  // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_SITL/Semaphores.h b/libraries/AP_HAL_SITL/Semaphores.h
index 63aa57f2f5..c88d76854f 100644
--- a/libraries/AP_HAL_SITL/Semaphores.h
+++ b/libraries/AP_HAL_SITL/Semaphores.h
@@ -9,6 +9,7 @@
 
 class HALSITL::Semaphore : public AP_HAL::Semaphore {
 public:
+    friend class HALSITL::BinarySemaphore;
     Semaphore();
     bool give() override;
     bool take(uint32_t timeout_ms) override;
@@ -24,3 +25,20 @@ protected:
     // semaphore once we're done with it
     uint8_t take_count;
 };
+
+
+class HALSITL::BinarySemaphore : public AP_HAL::BinarySemaphore {
+public:
+    BinarySemaphore(bool initial_state=false);
+
+    CLASS_NO_COPY(BinarySemaphore);
+
+    bool wait(uint32_t timeout_us) override;
+    bool wait_blocking(void) override;
+    void signal(void) override;
+
+private:
+    HALSITL::Semaphore mtx;
+    pthread_cond_t cond;
+    bool pending;
+};