From 982cff06959713cc28d3e3667d0f6e292a8324f0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Sun, 19 Jan 2020 08:57:24 +1100
Subject: [PATCH] AP_HAL_ChibiOS: make all semaphores recursive

the cost is very similar and this prevents an easy coding error which
can occur on less used code paths
---
 .../AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h |  1 -
 libraries/AP_HAL_ChibiOS/Semaphores.cpp       | 73 -------------------
 libraries/AP_HAL_ChibiOS/Semaphores.h         | 16 +---
 libraries/AP_HAL_ChibiOS/UARTDriver.h         |  2 +-
 .../AP_HAL_ChibiOS/hwdef/common/chconf.h      |  2 +-
 5 files changed, 4 insertions(+), 90 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h
index c320b7edc1..d525fb68e6 100644
--- a/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h
+++ b/libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h
@@ -13,7 +13,6 @@ namespace ChibiOS {
     class RCOutput;
     class Scheduler;
     class Semaphore;
-    class Semaphore_Recursive;
     class SPIBus;
     class SPIDesc;
     class SPIDevice;
diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.cpp b/libraries/AP_HAL_ChibiOS/Semaphores.cpp
index 06218b6e16..e09e641239 100644
--- a/libraries/AP_HAL_ChibiOS/Semaphores.cpp
+++ b/libraries/AP_HAL_ChibiOS/Semaphores.cpp
@@ -76,77 +76,4 @@ void Semaphore::assert_owner(void)
     osalDbgAssert(check_owner(), "owner");
 }
 
-// constructor
-Semaphore_Recursive::Semaphore_Recursive()
-    : Semaphore(), count(0)
-{}
-
-
-bool Semaphore_Recursive::give()
-{
-    chSysLock();
-    mutex_t *mtx = (mutex_t *)_lock;
-    osalDbgAssert(count>0, "recursive semaphore");
-    if (count != 0) {
-        count--;
-        if (count == 0) {
-            // this thread is giving it up
-            chMtxUnlockS(mtx);
-            // we may need to re-schedule if our priority was increased due to
-            // priority inheritance
-            chSchRescheduleS();
-        }
-    }
-    chSysUnlock();
-    return true;
-}
-
-bool Semaphore_Recursive::take(uint32_t timeout_ms)
-{
-    // most common case is we can get the lock immediately
-    if (Semaphore::take_nonblocking()) {
-        count=1;
-        return true;
-    }
-
-    // check for case where we hold it already
-    chSysLock();
-    mutex_t *mtx = (mutex_t *)_lock;
-    if (mtx->owner == chThdGetSelfX()) {
-        count++;
-        chSysUnlock();
-        return true;
-    }
-    chSysUnlock();
-    if (Semaphore::take(timeout_ms)) {
-        count = 1;
-        return true;
-    }
-    return false;
-}
-
-bool Semaphore_Recursive::take_nonblocking(void)
-{
-    // most common case is we can get the lock immediately
-    if (Semaphore::take_nonblocking()) {
-        count=1;
-        return true;
-    }
-
-    // check for case where we hold it already
-    chSysLock();
-    mutex_t *mtx = (mutex_t *)_lock;
-    if (mtx->owner == chThdGetSelfX()) {
-        count++;
-        chSysUnlock();
-        return true;
-    }
-    chSysUnlock();
-    if (Semaphore::take_nonblocking()) {
-        count = 1;
-        return true;
-    }
-    return false;
-}
-
 #endif // CH_CFG_USE_MUTEXES
diff --git a/libraries/AP_HAL_ChibiOS/Semaphores.h b/libraries/AP_HAL_ChibiOS/Semaphores.h
index 9ea8f93d0c..484570d086 100644
--- a/libraries/AP_HAL_ChibiOS/Semaphores.h
+++ b/libraries/AP_HAL_ChibiOS/Semaphores.h
@@ -34,18 +34,6 @@ public:
     void assert_owner(void);
 protected:
     // to avoid polluting the global namespace with the 'ch' variable,
-    // we declare the lock as a uint64_t, and cast inside the cpp file
-    uint64_t _lock[2];
-};
-
-// a recursive semaphore, allowing for a thread to take it more than
-// once. It must be released the same number of times it is taken
-class ChibiOS::Semaphore_Recursive : public ChibiOS::Semaphore {
-public:
-    Semaphore_Recursive();
-    bool give() override;
-    bool take(uint32_t timeout_ms) override;
-    bool take_nonblocking() override;
-private:
-    uint32_t count;
+    // we declare the lock as a uint32_t array, and cast inside the cpp file
+    uint32_t _lock[5];
 };
diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.h b/libraries/AP_HAL_ChibiOS/UARTDriver.h
index 87efeb5586..c7c3a1cd07 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.h
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.h
@@ -159,7 +159,7 @@ private:
 #endif
     ByteBuffer _readbuf{0};
     ByteBuffer _writebuf{0};
-    HAL_Semaphore_Recursive _write_mutex;
+    HAL_Semaphore _write_mutex;
 #ifndef HAL_UART_NODMA
     const stm32_dma_stream_t* rxdma;
     const stm32_dma_stream_t* txdma;
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h
index 6ac6bc960f..bac431c9a9 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h
@@ -266,7 +266,7 @@
  * @note    Requires @p CH_CFG_USE_MUTEXES.
  */
 #if !defined(CH_CFG_USE_MUTEXES_RECURSIVE)
-#define CH_CFG_USE_MUTEXES_RECURSIVE        FALSE
+#define CH_CFG_USE_MUTEXES_RECURSIVE        TRUE
 #endif
 
 /**