From bb43a8bfdc00346b07979d76429b3df934165ddf Mon Sep 17 00:00:00 2001
From: Thomas Watson <twatson52@icloud.com>
Date: Mon, 5 Aug 2024 10:57:38 -0500
Subject: [PATCH] AP_DroneCAN: DNA_Server: move allocation operation to db

Must be locked for the whole operation due to the find free/add
read-modify-write.

Preserves the previous behavior of sending back an ID of 0 in case of
allocation failure, for better or worse.
---
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp    | 33 ++++++++++++-------
 .../AP_DroneCAN/AP_DroneCAN_DNA_Server.h      |  3 ++
 2 files changed, 24 insertions(+), 12 deletions(-)

diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
index 89b14063ad035e..acf07859450b85 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp
@@ -512,18 +512,7 @@ void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer,
 
     if (rcvd_unique_id_offset == 16) {
         //We have received the full Unique ID, time to do allocation
-        uint8_t resp_node_id = db.getNodeIDForUniqueID((const uint8_t*)rcvd_unique_id, 16);
-        if (resp_node_id == 0) {
-            resp_node_id = db.findFreeNodeID(msg.node_id > MAX_NODE_ID ? 0 : msg.node_id);
-            if (resp_node_id != 0) {
-                db.addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16);
-                rsp.node_id = resp_node_id;
-            } else {
-                GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
-            }
-        } else {
-            rsp.node_id = resp_node_id;
-        }
+        rsp.node_id = db.handleAllocation(msg.node_id, (const uint8_t*)rcvd_unique_id);
         //reset states as well        
         rcvd_unique_id_offset = 0;
         memset(rcvd_unique_id, 0, sizeof(rcvd_unique_id));
@@ -532,6 +521,26 @@ void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer,
     allocation_pub.broadcast(rsp, false); // never publish allocation message with CAN FD
 }
 
+// handle the allocation message. returns the new node ID.
+uint8_t AP_DroneCAN_DNA_Server::Database::handleAllocation(uint8_t node_id, const uint8_t unique_id[])
+{
+    WITH_SEMAPHORE(sem);
+
+    uint8_t resp_node_id = getNodeIDForUniqueID(unique_id, 16);
+    if (resp_node_id == 0) {
+        resp_node_id = findFreeNodeID(node_id > MAX_NODE_ID ? 0 : node_id);
+        if (resp_node_id != 0) {
+            addNodeIDForUniqueID(resp_node_id, unique_id, 16);
+            return resp_node_id;
+        } else {
+            GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
+            return 0;
+        }
+    } else {
+        return resp_node_id;
+    }
+}
+
 //report the server state, along with failure message if any
 bool AP_DroneCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len) const
 {
diff --git a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
index 074ce4d7e819e2..13fef2ea6f5c92 100644
--- a/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
+++ b/libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h
@@ -43,6 +43,9 @@ class AP_DroneCAN_DNA_Server
         // handle processing the node info message. returns true if duplicate.
         bool handleNodeInfo(uint8_t source_node_id, const uint8_t unique_id[]);
 
+        // handle the allocation message. returns the new node ID.
+        uint8_t handleAllocation(uint8_t 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;