From bf860e8af19e8389c16c88280ae4eebda0ea25d4 Mon Sep 17 00:00:00 2001
From: Greg Harrison <ghexp@users.noreply.github.com>
Date: Sun, 5 Mar 2017 10:54:31 -0500
Subject: [PATCH] Workaround for missing sem_timedwait on os x

---
 MavLinkCom/include/MavLinkSemaphore.hpp | 11 ++-
 MavLinkCom/src/MavLinkSemaphore.cpp     | 94 ++++++++++++++++++++++++-
 2 files changed, 102 insertions(+), 3 deletions(-)

diff --git a/MavLinkCom/include/MavLinkSemaphore.hpp b/MavLinkCom/include/MavLinkSemaphore.hpp
index f5b70dd94d..8cd7d6b037 100644
--- a/MavLinkCom/include/MavLinkSemaphore.hpp
+++ b/MavLinkCom/include/MavLinkSemaphore.hpp
@@ -5,6 +5,10 @@
 #define MavLinkCom_Semaphore_hpp
 #include <memory>
 
+#ifdef __APPLE__
+#include <semaphore.h>
+#endif
+
 namespace mavlinkcom
 {
 	/*
@@ -27,13 +31,16 @@ namespace mavlinkcom
 		void wait();
 
 		// wait for a given number of milliseconds for one call to post.  Returns false if a timeout or EINTR occurs.
-		// If post has already been called then timed_wait returns immediately decrementing the count so the next 
+		// If post has already been called then timed_wait returns immediately decrementing the count so the next
 		// wait will block.  Throws exception if an error occurs.
 		bool timed_wait(int milliseconds);
-	private: 
+	private:
 		class semaphore_impl;
 		std::unique_ptr<semaphore_impl> impl_;
 	};
 }
 
 #endif
+#ifdef __APPLE__
+int sem_timedwait(sem_t *sem, const struct timespec *abs_timeout);
+#endif
diff --git a/MavLinkCom/src/MavLinkSemaphore.cpp b/MavLinkCom/src/MavLinkSemaphore.cpp
index aad0035674..f6802f663e 100644
--- a/MavLinkCom/src/MavLinkSemaphore.cpp
+++ b/MavLinkCom/src/MavLinkSemaphore.cpp
@@ -8,6 +8,10 @@
 using namespace mavlinkcom;
 using namespace common_utils;
 
+#ifdef __APPLE__
+#include <signal.h> //SIGALRM
+#endif
+
 #ifdef _WIN32
 #include <Windows.h>
 
@@ -117,7 +121,7 @@ class MavLinkSemaphore::semaphore_impl
 		absolute = absolute - seconds;
 		auto nanoSecondsRemaining = std::chrono::duration_cast<std::chrono::nanoseconds>(absolute);
 		struct timespec ts;
-		ts.tv_sec = seconds.count(); // seconds			
+		ts.tv_sec = seconds.count(); // seconds
 		milliseconds -= (ts.tv_sec * 1000);
 		ts.tv_nsec = nanoSecondsRemaining.count(); // nanoseconds
 		int rc = sem_timedwait(&semaphore, &ts);
@@ -163,3 +167,91 @@ bool MavLinkSemaphore::timed_wait(int millisecondTimeout)
 {
 	return impl_->timed_wait(millisecondTimeout);
 }
+
+#ifdef __APPLE__
+struct CSGX__sem_timedwait_Info
+{
+    pthread_mutex_t MxMutex;
+    pthread_cond_t MxCondition;
+    pthread_t MxParent;
+    struct timespec MxTimeout;
+    bool MxSignaled;
+};
+
+void *CSGX__sem_timedwait_Child(void *MainPtr)
+{
+    CSGX__sem_timedwait_Info *TempInfo = (CSGX__sem_timedwait_Info *)MainPtr;
+
+    pthread_mutex_lock(&TempInfo->MxMutex);
+
+    // Wait until the timeout or the condition is signaled, whichever comes first.
+    int Result;
+    do
+    {
+        Result = pthread_cond_timedwait(&TempInfo->MxCondition, &TempInfo->MxMutex, &TempInfo->MxTimeout);
+        if (!Result)  break;
+    } while (1);
+    if (errno == ETIMEDOUT && !TempInfo->MxSignaled)
+    {
+        TempInfo->MxSignaled = true;
+        pthread_kill(TempInfo->MxParent, SIGALRM);
+    }
+
+    pthread_mutex_unlock(&TempInfo->MxMutex);
+
+    return NULL;
+}
+
+int sem_timedwait(sem_t *sem, const struct timespec *abs_timeout)
+{
+    // Quick test to see if a lock can be immediately obtained.
+    int Result;
+
+    do
+    {
+        Result = sem_trywait(sem);
+        if (!Result)  return 0;
+    } while (Result < 0 && errno == EINTR);
+
+    // Since it couldn't be obtained immediately, it is time to shuttle the request off to a thread.
+    // Depending on the timeout, this could take longer than the timeout.
+    CSGX__sem_timedwait_Info TempInfo;
+
+    pthread_mutex_init(&TempInfo.MxMutex, NULL);
+    pthread_cond_init(&TempInfo.MxCondition, NULL);
+    TempInfo.MxParent = pthread_self();
+    TempInfo.MxTimeout.tv_sec = abs_timeout->tv_sec;
+    TempInfo.MxTimeout.tv_nsec = abs_timeout->tv_nsec;
+    TempInfo.MxSignaled = false;
+
+    sig_t OldSigHandler = signal(SIGALRM, SIG_DFL);
+
+    pthread_t ChildThread;
+    pthread_create(&ChildThread, NULL, CSGX__sem_timedwait_Child, &TempInfo);
+
+    // Wait for the semaphore, the timeout to expire, or an unexpected error condition.
+    do
+    {
+        Result = sem_wait(sem);
+        if (Result == 0 || TempInfo.MxSignaled || (Result < 0 && errno != EINTR))  break;
+    } while (1);
+
+    // Terminate the thread (if it is still running).
+    TempInfo.MxSignaled = true;
+    int LastError = errno;
+
+    pthread_mutex_lock(&TempInfo.MxMutex);
+    pthread_cond_signal(&TempInfo.MxCondition);
+    pthread_mutex_unlock(&TempInfo.MxMutex);
+    pthread_join(ChildThread, NULL);
+    pthread_cond_destroy(&TempInfo.MxCondition);
+    pthread_mutex_destroy(&TempInfo.MxMutex);
+
+    // Restore previous signal handler.
+    signal(SIGALRM, OldSigHandler);
+
+    errno = LastError;
+
+    return Result;
+}
+#endif