AP_DroneCAN: DNA_Server: clean up storage failure handling

The StorageManager read_block/write_block methods only return failure if
an out of bounds access is performed. Assert statically that this does
not happen.

Also remove the now-impossible failed to add node state.

libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.cpp

@@ -33,6 +33,7 @@ extern const AP_HAL::HAL& hal;
 #define NODEDATA_MAGIC 0xAC01
 #define NODEDATA_MAGIC_LEN 2
 #define MAX_NODE_ID    125
+#define NODEDATA_LOC(node_id) ((node_id * sizeof(struct NodeData)) + NODEDATA_MAGIC_LEN)
 
 #define debug_dronecan(level_debug, fmt, args...) do { AP::can().log_text(level_debug, "DroneCAN", fmt, ##args); } while (0)
 
@@ -43,7 +44,10 @@ AP_DroneCAN_DNA_Server::AP_DroneCAN_DNA_Server(AP_DroneCAN &ap_dronecan, CanardI
     allocation_sub(allocation_cb, driver_index),
     node_status_sub(node_status_cb, driver_index),
     node_info_client(_canard_iface, node_info_cb)
-{}
+{
+    // storage size must be synced with StorageCANDNA entry in StorageManager.cpp
+    static_assert(NODEDATA_LOC(MAX_NODE_ID+1) <= 1024, "DNA storage too small");
+}
 
 /* Method to generate 6byte hash from the Unique ID.
 We return it packed inside the referenced NodeData structure */
@@ -62,52 +66,43 @@ void AP_DroneCAN_DNA_Server::getHash(NodeData &node_data, const uint8_t unique_i
 }
 
 //Read Node Data from Storage Region
-bool AP_DroneCAN_DNA_Server::readNodeData(NodeData &data, uint8_t node_id)
+void AP_DroneCAN_DNA_Server::readNodeData(NodeData &data, uint8_t node_id)
 {
     if (node_id > MAX_NODE_ID) {
-        return false;
+        return;
     }
+
     WITH_SEMAPHORE(storage_sem);
-    if (!storage.read_block(&data, (node_id * sizeof(struct NodeData)) + NODEDATA_MAGIC_LEN, sizeof(struct NodeData))) {
+    storage.read_block(&data, NODEDATA_LOC(node_id), sizeof(struct NodeData));
-        //This will fall through to Prearm Check
-        server_state = STORAGE_FAILURE;
-        return false;
-    }
-    return true;
 }
 
 //Write Node Data to Storage Region
-bool AP_DroneCAN_DNA_Server::writeNodeData(const NodeData &data, uint8_t node_id)
+void AP_DroneCAN_DNA_Server::writeNodeData(const NodeData &data, uint8_t node_id)
 {
     if (node_id > MAX_NODE_ID) {
-        return false;
+        return;
     }
+
     WITH_SEMAPHORE(storage_sem);
-    if (!storage.write_block((node_id * sizeof(struct NodeData)) + NODEDATA_MAGIC_LEN,
+    storage.write_block(NODEDATA_LOC(node_id), &data, sizeof(struct NodeData));
-                             &data, sizeof(struct NodeData))) {
-        server_state = STORAGE_FAILURE;
-        return false;
-    }
-    return true;
 }
 
 /* Set Occupation Mask, handy for keeping track of all node ids that
 are allocated and all that are available. */
-bool AP_DroneCAN_DNA_Server::setOccupationMask(uint8_t node_id)
+void AP_DroneCAN_DNA_Server::setOccupationMask(uint8_t node_id)
 {
     if (node_id > MAX_NODE_ID) {
-        return false;
+        return;
     }
     occupation_mask.set(node_id);
-    return true;
 }
 
 /* Remove Node Data from Server Record in Storage,
 and also clear Occupation Mask */
-bool AP_DroneCAN_DNA_Server::freeNodeID(uint8_t node_id)
+void AP_DroneCAN_DNA_Server::freeNodeID(uint8_t node_id)
 {
     if (node_id > MAX_NODE_ID) {
-        return false;
+        return;
     }
 
     struct NodeData node_data;
@@ -118,8 +113,6 @@ bool AP_DroneCAN_DNA_Server::freeNodeID(uint8_t node_id)
 
     //Clear Occupation Mask
     occupation_mask.clear(node_id);
-
-    return true;
 }
 
 /* Sets the verification mask. This is to be called, once
@@ -166,9 +159,7 @@ uint8_t AP_DroneCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[],
         if (!isNodeIDOccupied(i)) { // No point in checking NodeID that's not taken
             continue;
         }
-        if (!readNodeData(node_data, i)) {
+        readNodeData(node_data, i);
-            break;  //Storage module has failed, report that as no NodeID detected
-        }
         if (memcmp(node_data.hwid_hash, cmp_node_data.hwid_hash, sizeof(NodeData::hwid_hash)) == 0) {
             node_id = i;
             break;
@@ -179,7 +170,7 @@ uint8_t AP_DroneCAN_DNA_Server::getNodeIDForUniqueID(const uint8_t unique_id[],
 
 /* Hash the Unique ID and add it to the Server Record
 for specified Node ID. */
-bool AP_DroneCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
+void AP_DroneCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size)
 {
     NodeData node_data;
     getHash(node_data, unique_id, size);
@@ -187,14 +178,9 @@ bool AP_DroneCAN_DNA_Server::addNodeIDForUniqueID(uint8_t node_id, const uint8_t
     node_data.crc = crc_crc8(node_data.hwid_hash, sizeof(node_data.hwid_hash));
 
     //Write Data to the records
-    if (!writeNodeData(node_data, node_id)) {
+    writeNodeData(node_data, node_id);
-        server_state = FAILED_TO_ADD_NODE;
-        fault_node_id = node_id;
-        return false;
-    }
 
     setOccupationMask(node_id);
-    return true;
 }
 
 //Checks if a valid Server Record is present for specified Node ID
@@ -255,9 +241,7 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id
                 reset_done = true;
             }
             //Add ourselves to the Server Record
-            if (!addNodeIDForUniqueID(node_id, own_unique_id, own_unique_id_len)) {
+            addNodeIDForUniqueID(node_id, own_unique_id, own_unique_id_len);
-                return false;
-            }
         }
     } else {
         //We have no record of our own Unique ID do a reset
@@ -268,9 +252,7 @@ bool AP_DroneCAN_DNA_Server::init(uint8_t own_unique_id[], uint8_t own_unique_id
             reset_done = true;
         }
         //Add ourselves to the Server Record
-        if (!addNodeIDForUniqueID(node_id, own_unique_id, own_unique_id_len)) {
+        addNodeIDForUniqueID(node_id, own_unique_id, own_unique_id_len);
-            return false;
-        }
     }
     /* Also add to seen node id this is to verify
     if any duplicates are on the bus carrying our Node ID */
@@ -565,9 +547,8 @@ void AP_DroneCAN_DNA_Server::handleAllocation(const CanardRxTransfer& transfer,
         if (resp_node_id == 255) {
             resp_node_id = findFreeNodeID(msg.node_id);
             if (resp_node_id != 255) {
-                if (addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16)) {
+                addNodeIDForUniqueID(resp_node_id, (const uint8_t*)rcvd_unique_id, 16);
-                    rsp.node_id = resp_node_id;
+                rsp.node_id = resp_node_id;
-                }
             } else {
                 GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "UC Node Alloc Failed!");
             }
@@ -588,10 +569,6 @@ bool AP_DroneCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len)
     switch (server_state) {
     case HEALTHY:
         return true;
-    case STORAGE_FAILURE: {
-        snprintf(fail_msg, fail_msg_len, "Failed to access storage!");
-        return false;
-    }
     case DUPLICATE_NODES: {
         if (_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_DUPLICATE_NODE)) {
             // ignore error
@@ -600,10 +577,6 @@ bool AP_DroneCAN_DNA_Server::prearm_check(char* fail_msg, uint8_t fail_msg_len)
         snprintf(fail_msg, fail_msg_len, "Duplicate Node %s../%d!", fault_node_name, fault_node_id);
         return false;
     }
-    case FAILED_TO_ADD_NODE: {
-        snprintf(fail_msg, fail_msg_len, "Failed to add Node %d!", fault_node_id);
-        return false;
-    }
     case NODE_STATUS_UNHEALTHY: {
         if (_ap_dronecan.option_is_set(AP_DroneCAN::Options::DNA_IGNORE_UNHEALTHY_NODE)) {
             // ignore error

libraries/AP_DroneCAN/AP_DroneCAN_DNA_Server.h

@@ -25,9 +25,7 @@ class AP_DroneCAN_DNA_Server
 
     enum ServerState {
         NODE_STATUS_UNHEALTHY = -5,
-        STORAGE_FAILURE = -3,
         DUPLICATE_NODES = -2,
-        FAILED_TO_ADD_NODE = -1,
         HEALTHY = 0
     };
 
@@ -66,15 +64,15 @@ class AP_DroneCAN_DNA_Server
     void reset();
 
     //Reads the Server Record from storage for specified node id
-    bool readNodeData(NodeData &data, uint8_t node_id);
+    void readNodeData(NodeData &data, uint8_t node_id);
 
     //Writes the Server Record from storage for specified node id
-    bool writeNodeData(const NodeData &data, uint8_t node_id);
+    void writeNodeData(const NodeData &data, uint8_t node_id);
 
     //Methods to set, clear and report NodeIDs allocated/registered so far
-    bool setOccupationMask(uint8_t node_id);
+    void setOccupationMask(uint8_t node_id);
     bool isNodeIDOccupied(uint8_t node_id) const;
-    bool freeNodeID(uint8_t node_id);
+    void freeNodeID(uint8_t node_id);
 
     //Set the mask to report that the unique id matches the record
     void setVerificationMask(uint8_t node_id);
@@ -83,7 +81,7 @@ class AP_DroneCAN_DNA_Server
     uint8_t getNodeIDForUniqueID(const uint8_t unique_id[], uint8_t size);
 
     //Add Node ID info to the record and setup necessary mask fields
-    bool addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
+    void addNodeIDForUniqueID(uint8_t node_id, const uint8_t unique_id[], uint8_t size);
 
     //Finds next available free Node, starting from preferred NodeID
     uint8_t findFreeNodeID(uint8_t preferred);