From 7d4cac5b49cd958f5ac6ccf1a9125c21a9d373d6 Mon Sep 17 00:00:00 2001
From: bugobliterator <siddharthbharatpurohit@gmail.com>
Date: Thu, 28 Dec 2023 08:31:27 +1100
Subject: [PATCH] canard: implement a lightweight way to share mutex between
 same message type subscribers

---
 canard/subscriber.h | 29 ++++++++++++++++++++++-------
 1 file changed, 22 insertions(+), 7 deletions(-)

diff --git a/canard/subscriber.h b/canard/subscriber.h
index a254801..5211eff 100644
--- a/canard/subscriber.h
+++ b/canard/subscriber.h
@@ -41,8 +41,17 @@ class Subscriber : public HandlerList {
     Subscriber(Callback<msgtype> &_cb, uint8_t _index) :
     HandlerList(CanardTransferTypeBroadcast, msgtype::cxx_iface::ID, msgtype::cxx_iface::SIGNATURE, _index),
     cb (_cb) {
+        if (branch_head[index] == nullptr) {
 #ifdef CANARD_MUTEX_ENABLED
-        WITH_SEMAPHORE(sem[index]);
+            sem = allocate<Canard::Semaphore>(); // allocate a semaphore
+#endif
+        } else {
+#ifdef CANARD_MUTEX_ENABLED
+            sem = branch_head[index]->sem;
+#endif
+        }
+#ifdef CANARD_MUTEX_ENABLED
+        WITH_SEMAPHORE(*sem);
 #endif
         next = branch_head[index];
         branch_head[index] = this;
@@ -53,9 +62,17 @@ class Subscriber : public HandlerList {
 
     // destructor, remove the entry from the singly-linked list
     ~Subscriber() {
+#ifdef CANARD_MUTEX_ENABLED
+        WITH_SEMAPHORE(*sem);
+#endif
         Subscriber<msgtype>* entry = branch_head[index];
         if (entry == this) {
             branch_head[index] = next;
+            if (branch_head[index] == nullptr) {
+#ifdef CANARD_MUTEX_ENABLED
+                deallocate(sem);
+#endif
+            }
             return;
         }
         while (entry != nullptr) {
@@ -70,6 +87,9 @@ class Subscriber : public HandlerList {
     /// @brief parse the message and call the callback
     /// @param transfer transfer object
     void handle_message(const CanardRxTransfer& transfer) override {
+#ifdef CANARD_MUTEX_ENABLED
+        WITH_SEMAPHORE(*sem);
+#endif
         msgtype msg {};
         msgtype::cxx_iface::decode(&transfer, &msg);
         // call all registered callbacks in one go
@@ -84,7 +104,7 @@ class Subscriber : public HandlerList {
     Subscriber<msgtype>* next;
     static Subscriber<msgtype> *branch_head[CANARD_NUM_HANDLERS];
 #ifdef CANARD_MUTEX_ENABLED
-    static Canard::Semaphore sem[CANARD_NUM_HANDLERS];
+    Canard::Semaphore *sem;
 #endif
     Callback<msgtype> &cb;
 };
@@ -92,11 +112,6 @@ class Subscriber : public HandlerList {
 template <typename msgtype>
 Subscriber<msgtype>* Subscriber<msgtype>::branch_head[] = {nullptr};
 
-#ifdef CANARD_MUTEX_ENABLED
-template <typename msgtype>
-Canard::Semaphore Subscriber<msgtype>::sem[CANARD_NUM_HANDLERS];
-#endif
-
 template <typename T, typename msgtype>
 class SubscriberArgCb {
 public: