HAL_ChibiOS: support recursive semaphores

libraries/AP_HAL_ChibiOS/AP_HAL_ChibiOS_Namespace.h

@@ -14,6 +14,7 @@ namespace ChibiOS {
     class RCOutput;
     class Scheduler;
     class Semaphore;
+    class Semaphore_Recursive;
     class SPIBus;
     class SPIDesc;
     class SPIDevice;

libraries/AP_HAL_ChibiOS/Semaphores.cpp

@@ -28,10 +28,8 @@ using namespace ChibiOS;
 Semaphore::Semaphore()
 {
     static_assert(sizeof(_lock) >= sizeof(mutex_t), "invalid mutex size");
-#if CH_CFG_USE_MUTEXES == TRUE
     mutex_t *mtx = (mutex_t *)_lock;
     chMtxObjectInit(mtx);
-#endif
 }
 
 bool Semaphore::give()
@@ -70,11 +68,7 @@ bool Semaphore::take_nonblocking()
 bool Semaphore::check_owner(void)
 {
     mutex_t *mtx = (mutex_t *)_lock;
-#if CH_CFG_USE_MUTEXES == TRUE
     return mtx->owner == chThdGetSelfX();
-#else
-    return true;
-#endif
 }
 
 void Semaphore::assert_owner(void)
@@ -82,5 +76,78 @@ 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
 

libraries/AP_HAL_ChibiOS/Semaphores.h

@@ -25,15 +25,27 @@
 class ChibiOS::Semaphore : public AP_HAL::Semaphore {
 public:
     Semaphore();
-    bool give() override;
-    bool take(uint32_t timeout_ms) override;
-    bool take_nonblocking() override;
+    virtual bool give() override;
+    virtual bool take(uint32_t timeout_ms) override;
+    virtual bool take_nonblocking() override;
 
     // methods within HAL_ChibiOS only
     bool check_owner(void);
     void assert_owner(void);
-private:
+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;
+};