From 05e04ee1a7643ba87358a37e24b427e63d196f7f Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 21 Dec 2015 07:33:11 +1100
Subject: [PATCH] HAL_SITL: added semaphore support

---
 libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h |  1 +
 libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h   |  1 +
 libraries/AP_HAL_SITL/Semaphores.cpp          | 39 +++++++++++++++++++
 libraries/AP_HAL_SITL/Semaphores.h            | 21 ++++++++++
 libraries/AP_HAL_SITL/Util.h                  |  4 ++
 5 files changed, 66 insertions(+)
 create mode 100644 libraries/AP_HAL_SITL/Semaphores.cpp
 create mode 100644 libraries/AP_HAL_SITL/Semaphores.h

diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
index 154394d187..f6cd7851d1 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Namespace.h
@@ -13,6 +13,7 @@ class SITLRCOutput;
 class ADCSource;
 class RCInput;
 class SITLUtil;
+class Semaphore;
 }
 
 #endif // __AP_HAL_SITL_NAMESPACE_H__
diff --git a/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h b/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h
index bdbe7c2c3e..a08c610197 100644
--- a/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h
+++ b/libraries/AP_HAL_SITL/AP_HAL_SITL_Private.h
@@ -7,6 +7,7 @@
 #include "Storage.h"
 #include "UARTDriver.h"
 #include "SITL_State.h"
+#include "Semaphores.h"
 
 #endif // __AP_HAL_SITL_PRIVATE_H__
 
diff --git a/libraries/AP_HAL_SITL/Semaphores.cpp b/libraries/AP_HAL_SITL/Semaphores.cpp
new file mode 100644
index 0000000000..5c46a07765
--- /dev/null
+++ b/libraries/AP_HAL_SITL/Semaphores.cpp
@@ -0,0 +1,39 @@
+#include <AP_HAL/AP_HAL.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+
+#include "Semaphores.h"
+
+extern const AP_HAL::HAL& hal;
+
+using namespace HALSITL;
+
+bool Semaphore::give() 
+{
+    return pthread_mutex_unlock(&_lock) == 0;
+}
+
+bool Semaphore::take(uint32_t timeout_ms) 
+{
+    if (timeout_ms == 0) {
+        return pthread_mutex_lock(&_lock) == 0;
+    }
+    if (take_nonblocking()) {
+        return true;
+    }
+    uint64_t start = AP_HAL::micros64();
+    do {
+        hal.scheduler->delay_microseconds(200);
+        if (take_nonblocking()) {
+            return true;
+        }
+    } while ((AP_HAL::micros64() - start) < timeout_ms*1000);
+    return false;
+}
+
+bool Semaphore::take_nonblocking() 
+{
+    return pthread_mutex_trylock(&_lock) == 0;
+}
+
+#endif // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_SITL/Semaphores.h b/libraries/AP_HAL_SITL/Semaphores.h
new file mode 100644
index 0000000000..b8f7b13781
--- /dev/null
+++ b/libraries/AP_HAL_SITL/Semaphores.h
@@ -0,0 +1,21 @@
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+#include "AP_HAL_SITL.h"
+#include <pthread.h>
+
+class HALSITL::Semaphore : public AP_HAL::Semaphore {
+public:
+    Semaphore() {
+        pthread_mutex_init(&_lock, NULL);
+    }
+    bool give();
+    bool take(uint32_t timeout_ms);
+    bool take_nonblocking();
+private:
+    pthread_mutex_t _lock;
+};
+#endif // CONFIG_HAL_BOARD
+
diff --git a/libraries/AP_HAL_SITL/Util.h b/libraries/AP_HAL_SITL/Util.h
index 55b28a7f92..22040f2c7c 100644
--- a/libraries/AP_HAL_SITL/Util.h
+++ b/libraries/AP_HAL_SITL/Util.h
@@ -4,6 +4,7 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include "AP_HAL_SITL_Namespace.h"
+#include "Semaphores.h"
 
 class HALSITL::SITLUtil : public AP_HAL::Util {
 public:
@@ -18,6 +19,9 @@ public:
         // SITL is assumed to always have plenty of memory. Return 128k for now
         return 0x20000;
     }
+
+    // create a new semaphore
+    AP_HAL::Semaphore *new_semaphore(void) override { return new HALSITL::Semaphore; }
 };
 
 #endif // __AP_HAL_SITL_UTIL_H__