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 +#ifdef __APPLE__ +#include +#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 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 //SIGALRM +#endif + #ifdef _WIN32 #include @@ -117,7 +121,7 @@ class MavLinkSemaphore::semaphore_impl absolute = absolute - seconds; auto nanoSecondsRemaining = std::chrono::duration_cast(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