added mavlink SECURE_COMMAND

RemoteIDModule/DroneCAN.cpp

@@ -698,66 +698,6 @@ void DroneCAN::handle_param_getset(CanardInstance* ins, CanardRxTransfer* transf
                            total_size);
 }
 
-/*
-  make a session key
- */
-void DroneCAN::make_session_key(uint8_t key[8]) const
-{
-    struct {
-        uint32_t time_us;
-        uint8_t mac[8];
-        uint32_t rand;
-    } data {};
-    static_assert(sizeof(data) % 4 == 0, "data must be multiple of 4 bytes");
-
-    esp_efuse_mac_get_default(data.mac);
-    data.time_us = micros();
-    data.rand = random(0xFFFFFFFF);
-    const uint64_t c64 = crc_crc64((const uint32_t *)&data, sizeof(data)/sizeof(uint32_t));
-    memcpy(key, (uint8_t *)&c64, 8);
-}
-
-/*
-  check signature in a command against bootloader public keys
- */
-bool DroneCAN::check_signature(const dronecan_remoteid_SecureCommandRequest &pkt)
-{
-    if (g.no_public_keys()) {
-        // allow through if no keys are setup
-        return true;
-    }
-    if (pkt.sig_length != 64) {
-        // monocypher signatures are 64 bytes
-        return false;
-    }
-    
-    /*
-      look over all public keys, if one matches then we are OK
-     */
-    for (uint8_t i=0; i<MAX_PUBLIC_KEYS; i++) {
-        uint8_t key[32];
-        if (!g.get_public_key(i, key)) {
-            continue;
-        }
-        crypto_check_ctx ctx {};
-        crypto_check_ctx_abstract *actx = (crypto_check_ctx_abstract*)&ctx;
-        const uint8_t data_length = pkt.data.len - pkt.sig_length;
-        crypto_check_init(actx, &pkt.data.data[data_length], key);
-
-        crypto_check_update(actx, (const uint8_t*)&pkt.sequence, sizeof(pkt.sequence));
-        crypto_check_update(actx, (const uint8_t*)&pkt.operation, sizeof(pkt.operation));
-        crypto_check_update(actx, pkt.data.data, data_length);
-        if (pkt.operation != DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_GET_REMOTEID_SESSION_KEY) {
-            crypto_check_update(actx, session_key, sizeof(session_key));
-        }
-        if (crypto_check_final(actx) == 0) {
-            // good signature
-            return true;
-        }
-    }
-    return false;
-}
-
 /*
   handle SecureCommand
  */
@@ -773,7 +713,8 @@ void DroneCAN::handle_SecureCommand(CanardInstance* ins, CanardRxTransfer* trans
     reply.sequence = req.sequence;
     reply.operation = req.operation;
 
-    if (!check_signature(req)) {
+    if (!check_signature(req.sig_length, req.data.len-req.sig_length,
+                         req.sequence, req.operation, req.data.data)) {
         reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_DENIED;
         goto send_reply;
     }

RemoteIDModule/DroneCAN.h

@@ -49,7 +49,6 @@ private:
     uint32_t send_next_node_id_allocation_request_at_ms;
     uint32_t node_id_allocation_unique_id_offset;
     uint32_t last_DNA_start_ms;
-    uint8_t session_key[8];
 
     uavcan_protocol_NodeStatus node_status;
 
@@ -62,8 +61,6 @@ private:
     void handle_SecureCommand(CanardInstance* ins, CanardRxTransfer* transfer);
 
     void can_printf(const char *fmt, ...);
-    void make_session_key(uint8_t key[8]) const;
-    bool check_signature(const dronecan_remoteid_SecureCommandRequest &pkt);
 
 public:
     void onTransferReceived(CanardInstance* ins, CanardRxTransfer* transfer);

RemoteIDModule/mavlink.cpp

@@ -233,6 +233,13 @@ void MAVLinkSerial::process_packet(mavlink_status_t &status, mavlink_message_t &
                                      g.param_index_float(p));
         break;
     }
+    case MAVLINK_MSG_ID_SECURE_COMMAND:
+    case MAVLINK_MSG_ID_SECURE_COMMAND_REPLY: {
+        mavlink_secure_command_t pkt;
+        mavlink_msg_secure_command_decode(&msg, &pkt);
+        handle_secure_command(pkt);
+        break;
+    }
     default:
         // we don't care about other packets
         break;

RemoteIDModule/mavlink.h

@@ -27,6 +27,7 @@ private:
     void update_send(void);
     void process_packet(mavlink_status_t &status, mavlink_message_t &msg);
     void mav_printf(uint8_t severity, const char *fmt, ...);
+    void handle_secure_command(const mavlink_secure_command_t &pkt);
 
     void arm_status_send(void);
 };

RemoteIDModule/mavlink_secure_command.cpp

@@ -0,0 +1,116 @@
+/*
+  mavlink class for handling SECURE_COMMAND messages
+ */
+#include <Arduino.h>
+#include "mavlink.h"
+#include "board_config.h"
+#include "version.h"
+#include "parameters.h"
+
+/*
+  handle a SECURE_COMMAND
+ */
+void MAVLinkSerial::handle_secure_command(const mavlink_secure_command_t &pkt)
+{
+    mavlink_secure_command_reply_t reply {};
+    reply.result = MAV_RESULT_UNSUPPORTED;
+    reply.sequence = pkt.sequence;
+    reply.operation = pkt.operation;
+
+    if (uint16_t(pkt.data_length) + uint16_t(pkt.sig_length) > sizeof(pkt.data)) {
+        reply.result = MAV_RESULT_DENIED;
+        goto send_reply;
+    }
+    if (!check_signature(pkt.sig_length, pkt.data_length, pkt.sequence, pkt.operation, pkt.data)) {
+        reply.result = MAV_RESULT_DENIED;
+        goto send_reply;
+    }
+
+    switch (pkt.operation) {
+
+    case SECURE_COMMAND_GET_SESSION_KEY: {
+        make_session_key(session_key);
+        reply.data_length = sizeof(session_key);
+        memcpy(reply.data, session_key, reply.data_length);
+        reply.result = MAV_RESULT_ACCEPTED;
+        break;
+    }
+
+    case SECURE_COMMAND_GET_PUBLIC_KEYS: {
+        if (pkt.data_length != 2) {
+            reply.result = MAV_RESULT_UNSUPPORTED;
+            goto send_reply;
+        }
+        const uint8_t key_idx = pkt.data[0];
+        uint8_t num_keys = pkt.data[1];
+        const uint8_t max_fetch = (sizeof(reply.data)-1) / PUBLIC_KEY_LEN;
+        if (key_idx >= MAX_PUBLIC_KEYS ||
+            num_keys > max_fetch ||
+            key_idx+num_keys > MAX_PUBLIC_KEYS ||
+            g.no_public_keys()) {
+            reply.result = MAV_RESULT_FAILED;
+            goto send_reply;
+        }
+
+        for (uint8_t i=0;i<num_keys;i++) {
+            g.get_public_key(i+key_idx, &reply.data[1+i*PUBLIC_KEY_LEN]);
+        }
+
+        reply.data_length = 1+num_keys*PUBLIC_KEY_LEN;
+        reply.data[0] = key_idx;
+        reply.result = MAV_RESULT_ACCEPTED;
+        break;
+    }
+
+    case SECURE_COMMAND_SET_PUBLIC_KEYS: {
+        if (pkt.data_length < PUBLIC_KEY_LEN+1) {
+            reply.result = MAV_RESULT_FAILED;
+            goto send_reply;
+        }
+        const uint8_t key_idx = pkt.data[0];
+        const uint8_t num_keys = (pkt.data_length-1) / PUBLIC_KEY_LEN;
+        if (num_keys == 0) {
+            reply.result = MAV_RESULT_FAILED;
+            goto send_reply;
+        }
+        if (key_idx >= MAX_PUBLIC_KEYS ||
+            key_idx+num_keys > MAX_PUBLIC_KEYS) {
+            reply.result = MAV_RESULT_FAILED;
+            goto send_reply;
+        }
+        bool failed = false;
+        for (uint8_t i=0; i<num_keys; i++) {
+            failed |= !g.set_public_key(key_idx+i, &pkt.data[1+i*PUBLIC_KEY_LEN]);
+        }
+        reply.result = failed? MAV_RESULT_FAILED : MAV_RESULT_ACCEPTED;
+        break;
+    }
+
+    case SECURE_COMMAND_REMOVE_PUBLIC_KEYS: {
+        if (pkt.data_length != 2) {
+            reply.result = MAV_RESULT_FAILED;
+            goto send_reply;
+        }
+        const uint8_t key_idx = pkt.data[0];
+        const uint8_t num_keys = pkt.data[1];
+        if (num_keys == 0) {
+            reply.result = MAV_RESULT_FAILED;
+            goto send_reply;
+        }
+        if (key_idx >= MAX_PUBLIC_KEYS ||
+            key_idx+num_keys > MAX_PUBLIC_KEYS) {
+            reply.result = MAV_RESULT_FAILED;
+            goto send_reply;
+        }
+        for (uint8_t i=0; i<num_keys; i++) {
+            g.remove_public_key(key_idx+i);
+        }
+        reply.result = MAV_RESULT_ACCEPTED;
+        break;
+    }
+    }
+
+send_reply:
+    // send reply
+    mavlink_msg_secure_command_reply_send_struct(chan, &reply);
+}

RemoteIDModule/parameters.cpp

@@ -400,3 +400,29 @@ bool Parameters::no_public_keys(void) const
     }
     return true;
 }
+
+bool Parameters::set_public_key(uint8_t i, const uint8_t key[32])
+{
+    if (i >= MAX_PUBLIC_KEYS) {
+        return false;
+    }
+    char *s = base64_encode(key, PUBLIC_KEY_LEN);
+    if (s == nullptr) {
+        return false;
+    }
+    char name[] = "PUBLIC_KEYx";
+    name[strlen(name)-2] = '1'+i;
+    bool ret = set_by_name_char64(name, s);
+    delete[] s;
+    return ret;
+}
+
+bool Parameters::remove_public_key(uint8_t i)
+{
+    if (i >= MAX_PUBLIC_KEYS) {
+        return false;
+    }
+    char name[] = "PUBLIC_KEYx";
+    name[strlen(name)-2] = '1'+i;
+    return set_by_name_char64(name, "");
+}

RemoteIDModule/parameters.h

@@ -82,6 +82,8 @@ public:
       return a public key
     */
     bool get_public_key(uint8_t i, uint8_t key[32]) const;
+    bool set_public_key(uint8_t i, const uint8_t key[32]);
+    bool remove_public_key(uint8_t i);
     bool no_public_keys(void) const;
 
     static uint16_t param_count_float(void);

RemoteIDModule/transport.cpp

@@ -4,6 +4,8 @@
 #include <Arduino.h>
 #include "transport.h"
 #include "parameters.h"
+#include "util.h"
+#include "monocypher.h"
 
 const char *Transport::parse_fail = "uninitialised";
 
@@ -57,3 +59,65 @@ uint8_t Transport::arm_status_check(const char *&reason)
 
     return status;
 }
+
+/*
+  make a session key
+ */
+void Transport::make_session_key(uint8_t key[8]) const
+{
+    struct {
+        uint32_t time_us;
+        uint8_t mac[8];
+        uint32_t rand;
+    } data {};
+    static_assert(sizeof(data) % 4 == 0, "data must be multiple of 4 bytes");
+
+    esp_efuse_mac_get_default(data.mac);
+    data.time_us = micros();
+    data.rand = random(0xFFFFFFFF);
+    const uint64_t c64 = crc_crc64((const uint32_t *)&data, sizeof(data)/sizeof(uint32_t));
+    memcpy(key, (uint8_t *)&c64, 8);
+}
+
+/*
+  check signature in a command against public keys
+ */
+bool Transport::check_signature(uint8_t sig_length, uint8_t data_len, uint32_t sequence, uint32_t operation,
+                                const uint8_t *data)
+{
+    if (g.no_public_keys()) {
+        // allow through if no keys are setup
+        return true;
+    }
+    if (sig_length != 64) {
+        // monocypher signatures are 64 bytes
+        return false;
+    }
+    
+    /*
+      loop over all public keys, if one matches then we are OK
+     */
+    for (uint8_t i=0; i<MAX_PUBLIC_KEYS; i++) {
+        uint8_t key[32];
+        if (!g.get_public_key(i, key)) {
+            continue;
+        }
+        crypto_check_ctx ctx {};
+        crypto_check_ctx_abstract *actx = (crypto_check_ctx_abstract*)&ctx;
+        crypto_check_init(actx, &data[data_len], key);
+
+        crypto_check_update(actx, (const uint8_t*)&sequence, sizeof(sequence));
+        crypto_check_update(actx, (const uint8_t*)&operation, sizeof(operation));
+        crypto_check_update(actx, data, data_len);
+        if (operation != SECURE_COMMAND_GET_SESSION_KEY &&
+            operation != SECURE_COMMAND_GET_REMOTEID_SESSION_KEY) {
+            crypto_check_update(actx, session_key, sizeof(session_key));
+        }
+        if (crypto_check_final(actx) == 0) {
+            // good signature
+            return true;
+        }
+    }
+    return false;
+}
+

RemoteIDModule/transport.h

@@ -72,4 +72,14 @@ protected:
     static mavlink_open_drone_id_self_id_t self_id;
     static mavlink_open_drone_id_system_t system;
     static mavlink_open_drone_id_operator_id_t operator_id;
+
+    void make_session_key(uint8_t key[8]) const;
+
+    /*
+      check signature in a command against public keys
+    */
+    bool check_signature(uint8_t sig_length, uint8_t data_len, uint32_t sequence, uint32_t operation,
+                         const uint8_t *data);
+
+    uint8_t session_key[8];
 };

RemoteIDModule/util.cpp

@@ -27,13 +27,13 @@ uint64_t crc_crc64(const uint32_t *data, uint16_t num_words)
     return crc;
 }
 
+static const char b64[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
+
 /*
   simple base64 decoder, not particularly efficient, but small
  */
 int32_t base64_decode(const char *s, uint8_t *out, const uint32_t max_len)
 {
-    static const char b64[] = "ABCDEFGHIJKLMNOPQRSTUVWXYZabcdefghijklmnopqrstuvwxyz0123456789+/";
-
     const char *p;
     uint32_t n = 0;
     uint32_t i = 0;
@@ -70,4 +70,38 @@ int32_t base64_decode(const char *s, uint8_t *out, const uint32_t max_len)
     return n;
 }
 
+/*
+  encode as base64, returning allocated string
+*/
+char *base64_encode(const uint8_t *d, int len)
+{
+    uint32_t bit_offset, byte_offset, idx, i;
+    uint32_t bytes = (len*8 + 5)/6;
+    uint32_t pad_bytes = (bytes % 4) ? 4 - (bytes % 4) : 0;
+
+    char *out = new char[bytes+pad_bytes+1];
+    if (!out) {
+        return nullptr;
+    }
+
+    for (i=0;i<bytes;i++) {
+        byte_offset = (i*6)/8;
+        bit_offset = (i*6)%8;
+        if (bit_offset < 3) {
+            idx = (d[byte_offset] >> (2-bit_offset)) & 0x3FU;
+        } else {
+            idx = (d[byte_offset] << (bit_offset-2)) & 0x3FU;
+            if (byte_offset+1 < len) {
+                idx |= (d[byte_offset+1] >> (8-(bit_offset-2)));
+            }
+        }
+        out[i] = b64[idx];
+    }
+
+    for (;i<bytes+pad_bytes;i++) {
+        out[i] = '=';
+    }
+    out[i] = 0;
 
+    return out;
+}

RemoteIDModule/util.h

@@ -17,3 +17,9 @@ uint64_t crc_crc64(const uint32_t *data, uint16_t num_words);
   decode a base64 string
  */
 int32_t base64_decode(const char *s, uint8_t *out, const uint32_t max_len);
+
+/*
+  encode as base64, returning allocated string
+*/
+char *base64_encode(const uint8_t *buf, int len);
+