From a9a2c8c392ebac465777d2af206aed5e58684853 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Thu, 12 Nov 2020 18:45:09 +1100
Subject: [PATCH] AP_HAL_SITL: only disown a sempahore once we're done with it

---
 libraries/AP_HAL_SITL/Semaphores.cpp | 13 +++++++++++--
 libraries/AP_HAL_SITL/Semaphores.h   |  3 +++
 2 files changed, 14 insertions(+), 2 deletions(-)

diff --git a/libraries/AP_HAL_SITL/Semaphores.cpp b/libraries/AP_HAL_SITL/Semaphores.cpp
index 9c1f155cdd..3bd22e9500 100644
--- a/libraries/AP_HAL_SITL/Semaphores.cpp
+++ b/libraries/AP_HAL_SITL/Semaphores.cpp
@@ -21,10 +21,13 @@ Semaphore::Semaphore()
 
 bool Semaphore::give()
 {
+    take_count--;
     if (pthread_mutex_unlock(&_lock) != 0) {
         AP_HAL::panic("Bad semaphore usage");
     }
-    owner = (pthread_t)-1;
+    if (take_count == 0) {
+        owner = (pthread_t)-1;
+    }
     return true;
 }
 
@@ -41,6 +44,7 @@ bool Semaphore::take(uint32_t timeout_ms)
     if (timeout_ms == HAL_SEMAPHORE_BLOCK_FOREVER) {
         if (pthread_mutex_lock(&_lock) == 0) {
             owner = pthread_self();
+            take_count++;
             return true;
         }
         return false;
@@ -64,7 +68,12 @@ bool Semaphore::take(uint32_t timeout_ms)
 
 bool Semaphore::take_nonblocking()
 {
-    return pthread_mutex_trylock(&_lock) == 0;
+    if (pthread_mutex_trylock(&_lock) == 0) {
+        owner = pthread_self();
+        take_count++;
+        return true;
+    }
+    return false;
 }
 
 #endif  // CONFIG_HAL_BOARD
diff --git a/libraries/AP_HAL_SITL/Semaphores.h b/libraries/AP_HAL_SITL/Semaphores.h
index 6fd1d9178a..75abb612e4 100644
--- a/libraries/AP_HAL_SITL/Semaphores.h
+++ b/libraries/AP_HAL_SITL/Semaphores.h
@@ -20,4 +20,7 @@ protected:
     pthread_mutex_t _lock;
     pthread_t owner;
 
+    // keep track the recursion level to ensure we only disown the
+    // semaphore once we're done with it
+    uint8_t take_count;
 };