From 552a06dab737a50a74aaa67441120b271f211348 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Thu, 16 Jan 2020 10:49:41 +1100
Subject: [PATCH] AP_UAVCAN: protect UAVCAN DNA server with semaphore

---
 libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp | 7 +++++++
 libraries/AP_UAVCAN/AP_UAVCAN_Server.h   | 2 ++
 2 files changed, 9 insertions(+)

diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp b/libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp
index 25e808670c..4f1b242804 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_Server.cpp
@@ -258,6 +258,8 @@ bool AP_UAVCAN_Server::init(AP_UAVCAN *ap_uavcan)
         return false;
     }
 
+    WITH_SEMAPHORE(sem);
+
     //Read the details from ap_uavcan
     uavcan::Node<0>* _node = ap_uavcan->get_node();
     uint8_t node_id = _node->getNodeID().get();
@@ -418,6 +420,8 @@ seen list, So that we can raise issue if there are duplicates
 on the bus. */
 void AP_UAVCAN_Server::verify_nodes(AP_UAVCAN *ap_uavcan)
 {
+    WITH_SEMAPHORE(sem);
+
     uint32_t now = AP_HAL::millis();
     if ((now - last_verification_request) < 5000) {
         return;
@@ -468,6 +472,7 @@ void AP_UAVCAN_Server::handleNodeStatus(uint8_t node_id, const NodeStatusCb &cb)
     if (node_id > MAX_NODE_ID) {
         return;
     }
+    WITH_SEMAPHORE(sem);
     if (!isNodeIDVerified(node_id)) {
         //immediately begin verification of the node_id
         for (uint8_t i = 0; i < MAX_NUMBER_OF_CAN_DRIVERS; i++) {
@@ -502,6 +507,7 @@ void AP_UAVCAN_Server::handleNodeInfo(uint8_t node_id, uint8_t unique_id[], char
     if (node_id > MAX_NODE_ID) {
         return;
     }
+    WITH_SEMAPHORE(sem);
     if (isNodeIDOccupied(node_id)) {
         //if node_id already registered, just verify if Unique ID matches as well
         if (node_id == getNodeIDForUniqueID(unique_id, 16)) {
@@ -558,6 +564,7 @@ void AP_UAVCAN_Server::handleAllocation(uint8_t driver_index, uint8_t node_id, c
         //init has not been called for this driver.
         return;
     }
+    WITH_SEMAPHORE(sem);
     if (!cb.msg->isAnonymousTransfer()) {
         //Ignore Allocation messages that are not DNA requests
         return;
diff --git a/libraries/AP_UAVCAN/AP_UAVCAN_Server.h b/libraries/AP_UAVCAN/AP_UAVCAN_Server.h
index 29dddd2a69..bbfc13c7b3 100644
--- a/libraries/AP_UAVCAN/AP_UAVCAN_Server.h
+++ b/libraries/AP_UAVCAN/AP_UAVCAN_Server.h
@@ -82,6 +82,8 @@ class AP_UAVCAN_Server
     //Look in the storage and check if there's a valid Server Record there
     bool isValidNodeDataAvailable(uint8_t node_id);
 
+    HAL_Semaphore_Recursive sem;
+
 public:
     AP_UAVCAN_Server(StorageAccess _storage) : storage(_storage) {}