From f406c624490a596f1aa586acdf46b40e149f765e Mon Sep 17 00:00:00 2001
From: Thomas Watson <twatson52@icloud.com>
Date: Mon, 5 Aug 2024 10:50:55 -0500
Subject: [PATCH] AP_DroneCAN: DNA_Server: move node info update operation to
 db

Must be locked for the whole operation due to the occupied check and
addition read-modify-write.
---
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp    | 48 ++++++++++++-------
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.h      |  3 ++
 2 files changed, 34 insertions(+), 17 deletions(-)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
index deb5f8c8a2..ccdaaddca4 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
@@ -400,14 +400,9 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
     }
 #endif
 
-    if (db.isOccupied(transfer.source_node_id)) {
-        //if node_id already registered, just verify if Unique ID matches as well
-        if (transfer.source_node_id == db.getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16)) {
-            if (transfer.source_node_id == curr_verifying_node) {
-                nodeInfo_resp_rcvd = true;
-            }
-            node_verified.set(transfer.source_node_id);
-        } else if (!_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
+    bool duplicate = db.handleNodeInfo(transfer.source_node_id, rsp.hardware_version.unique_id);
+    if (duplicate) {
+        if (!_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
             /* This is a device with node_id already registered
             for another device */
             server_state = DUPLICATE_NODES;
@@ -415,15 +410,6 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
             memcpy(fault_node_name, rsp.name.data, sizeof(fault_node_name));
         }
     } else {
-        /* Node Id was not allocated by us, or during this boot, let's register this in our records
-        Check if we allocated this Node before */
-        uint8_t prev_node_id = db.getNodeIDForUniqueID(rsp.hardware_version.unique_id, 16);
-        if (prev_node_id != 0) {
-            //yes we did, remove this registration
-            db.freeNodeID(prev_node_id);
-        }
-        //add a new server record
-        db.addNodeIDForUniqueID(transfer.source_node_id, rsp.hardware_version.unique_id, 16);
         //Verify as well
         node_verified.set(transfer.source_node_id);
         if (transfer.source_node_id == curr_verifying_node) {
@@ -432,6 +418,34 @@ void AP_DroneCAN_DNA_Server::handleNodeInfo(const CanardRxTransfer& transfer, co
     }
 }
 
+// handle processing the node info message. returns true if duplicate.
+bool AP_DroneCAN_DNA_Server::Database::handleNodeInfo(uint8_t source_node_id, const uint8_t unique_id[])
+{
+    WITH_SEMAPHORE(sem);
+
+    if (isOccupied(source_node_id)) {
+        //if node_id already registered, just verify if Unique ID matches as well
+        if (source_node_id == getNodeIDForUniqueID(unique_id, 16)) {
+            return false;
+        } else {
+            /* This is a device with node_id already registered
+            for another device */
+            return true;
+        }
+    } else {
+        /* Node Id was not allocated by us, or during this boot, let's register this in our records
+        Check if we allocated this Node before */
+        uint8_t prev_node_id = getNodeIDForUniqueID(unique_id, 16);
+        if (prev_node_id != 0) {
+            //yes we did, remove this registration
+            freeNodeID(prev_node_id);
+        }
+        //add a new server record
+        addNodeIDForUniqueID(source_node_id, unique_id, 16);
+        return false;
+    }
+}
+
 /* Handle the allocation message from the devices supporting
 dynamic node allocation. */
 void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer, const uavcan_protocol_dynamic_node_id_Allocation& msg)
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
index a7ad68cc4d..a6c3d1bb88 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
@@ -41,6 +41,9 @@ class AP_DroneCAN_DNA_Server
         // handle initializing the server with the given expected node ID and unique ID
         void initServer(uint8_t node_id, const uint8_t own_unique_id[], uint8_t own_unique_id_len);
 
+        // handle processing the node info message. returns true if duplicate.
+        bool handleNodeInfo(uint8_t source_node_id, const uint8_t unique_id[]);
+
         //Generates 6Byte long hash from the specified unique_id
         void getHash(NodeData &node_data, const uint8_t unique_id[], uint8_t size) const;