canard++: separate out link() method

and allow child class access to the HandlerList mutex
dronecan · Feb 17, 2025 · 1715920 · 1715920
1 parent f4f84bf
commit 1715920
Show file tree

Hide file tree

Showing 3 changed files with 37 additions and 33 deletions.
diff --git 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;
@@ -128,14 +101,37 @@ class HandlerList {
     virtual void handle_message(const CanardRxTransfer& transfer) = 0;
 
 protected:
+    virtual ~HandlerList() {}
     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
@@ -41,15 +41,22 @@ 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
     Client(const Client&) = delete;
 
     // 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
@@ -46,13 +46,17 @@ class Subscriber : public HandlerList {
 #endif
         next = branch_head[index];
         branch_head[index] = this;
+        link();
     }
 
     // delete copy constructor and assignment operator
     Subscriber(const Subscriber&) = delete;
 
     // 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;
 };