AP_UAVCAN: protect UAVCAN DNA server with semaphore

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;

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) {}