From 414753b19be34d326201525551974d9907ca8e58 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 17 Feb 2025 16:06:35 +1100
Subject: [PATCH] canard++: separate out link() method

and allow child class access to the HandlerList mutex
---
 canard/handler_list.h   | 55 +++++++++++++++++++----------------------
 canard/service_client.h |  7 ++++++
 canard/subscriber.h     |  7 +++---
 3 files changed, 36 insertions(+), 33 deletions(-)

diff --git a/canard/handler_list.h b/canard/handler_list.h
index cea18a6..c9414da 100644
--- a/canard/handler_list.h
+++ b/canard/handler_list.h
@@ -43,14 +43,6 @@ class HandlerList {
     /// @param _index Index of the handler list
     HandlerList(CanardTransferType _transfer_type, uint16_t _msgid, uint64_t _signature, uint8_t _index) NOINLINE_FUNC :
     index(_index) {
-        if (index >= CANARD_NUM_HANDLERS) {
-            return;
-        }
-#ifdef WITH_SEMAPHORE
-        WITH_SEMAPHORE(sem[index]);
-#endif
-        next = head[index];
-        head[index] = this;
         msgid = _msgid;
         signature = _signature;
         transfer_type = _transfer_type;
@@ -59,25 +51,6 @@ class HandlerList {
     /// @brief delete copy constructor and assignment operator
     HandlerList(const HandlerList&) = delete;
 
-    // remove the entry from the singly-linked list
-    void unlink() NOINLINE_FUNC {
-#ifdef WITH_SEMAPHORE
-        WITH_SEMAPHORE(sem[index]);
-#endif
-        HandlerList* entry = head[index];
-        if (entry == this) {
-            head[index] = next;
-            return;
-        }
-        while (entry != nullptr) {
-            if (entry->next == this) {
-                entry->next = next;
-                return;
-            }
-            entry = entry->next;
-        }
-    }
-
 #ifdef WITH_SEMAPHORE
     static HAL_Semaphore *get_semaphore(uint8_t idx) {
         return idx < CANARD_NUM_HANDLERS? &sem[idx] : nullptr;
@@ -129,13 +102,35 @@ class HandlerList {
 
 protected:
     uint8_t index;
+#ifdef WITH_SEMAPHORE
+    static Canard::Semaphore sem[CANARD_NUM_HANDLERS];
+#endif
     HandlerList* next;
 
+    // add the entry into the linked list
+    void link(void) NOINLINE_FUNC {
+        next = head[index];
+        head[index] = this;
+    }
+
+    // remove the entry from the singly-linked list
+    void unlink() NOINLINE_FUNC {
+        HandlerList* entry = head[index];
+        if (entry == this) {
+            head[index] = next;
+            return;
+        }
+        while (entry != nullptr) {
+            if (entry->next == this) {
+                entry->next = next;
+                return;
+            }
+            entry = entry->next;
+        }
+    }
+    
 private:
     static HandlerList* head[CANARD_NUM_HANDLERS];
-#ifdef WITH_SEMAPHORE
-    static Canard::Semaphore sem[CANARD_NUM_HANDLERS];
-#endif
     uint16_t msgid;
     uint64_t signature;
     CanardTransferType transfer_type;
diff --git a/canard/service_client.h b/canard/service_client.h
index 02cb76c..0e39ea8 100644
--- a/canard/service_client.h
+++ b/canard/service_client.h
@@ -41,8 +41,12 @@ class Client : public HandlerList, public Sender {
     Sender(_interface),
     server_node_id(255),
     cb(_cb) {
+#ifdef WITH_SEMAPHORE
+        WITH_SEMAPHORE(sem[index]);
+#endif
         next = branch_head[index];
         branch_head[index] = this;
+        link();
     }
 
     // delete copy constructor and assignment operator
@@ -50,6 +54,9 @@ class Client : public HandlerList, public Sender {
 
     // destructor, remove the entry from the singly-linked list
     ~Client() {
+#ifdef WITH_SEMAPHORE
+        WITH_SEMAPHORE(sem[index]);
+#endif
         unlink();
         Client<rsptype>* entry = branch_head[index];
         if (entry == this) {
diff --git a/canard/subscriber.h b/canard/subscriber.h
index 87047ca..0f337b9 100644
--- a/canard/subscriber.h
+++ b/canard/subscriber.h
@@ -46,6 +46,7 @@ class Subscriber : public HandlerList {
 #endif
         next = branch_head[index];
         branch_head[index] = this;
+        link();
     }
 
     // delete copy constructor and assignment operator
@@ -53,6 +54,9 @@ class Subscriber : public HandlerList {
 
     // destructor, remove the entry from the singly-linked list
     ~Subscriber() {
+#ifdef WITH_SEMAPHORE
+        WITH_SEMAPHORE(sem[index]);
+#endif
         unlink();
         Subscriber<msgtype>* entry = branch_head[index];
         if (entry == this) {
@@ -87,9 +91,6 @@ class Subscriber : public HandlerList {
 private:
     Subscriber<msgtype>* next;
     static Subscriber<msgtype> *branch_head[CANARD_NUM_HANDLERS];
-#ifdef WITH_SEMAPHORE
-    Canard::Semaphore sem[CANARD_NUM_HANDLERS];
-#endif
     Callback<msgtype> &cb;
 };