support DroneCAN RemoteID SecureCommand

allows update of parameters securely

RemoteIDModule/DroneCAN.cpp

@@ -9,6 +9,8 @@
 #include "DroneCAN.h"
 #include "parameters.h"
 #include <stdarg.h>
+#include "util.h"
+#include "monocypher.h"
 
 #include <canard.h>
 #include <uavcan.protocol.NodeStatus.h>
@@ -154,6 +156,9 @@ void DroneCAN::onTransferReceived(CanardInstance* ins,
     case UAVCAN_PROTOCOL_PARAM_GETSET_ID:
         handle_param_getset(ins, transfer);
         break;
+    case DRONECAN_REMOTEID_SECURECOMMAND_ID:
+        handle_SecureCommand(ins, transfer);
+        break;
     default:
         //Serial.printf("reject %u\n", transfer->data_type_id);
         break;
@@ -181,6 +186,7 @@ bool DroneCAN::shouldAcceptTransfer(const CanardInstance* ins,
         ACCEPT_ID(DRONECAN_REMOTEID_SELFID);
         ACCEPT_ID(DRONECAN_REMOTEID_OPERATORID);
         ACCEPT_ID(DRONECAN_REMOTEID_SYSTEM);
+        ACCEPT_ID(DRONECAN_REMOTEID_SECURECOMMAND);
         ACCEPT_ID(UAVCAN_PROTOCOL_PARAM_GETSET);
         return true;
     }
@@ -687,6 +693,135 @@ 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
+ */
+void DroneCAN::handle_SecureCommand(CanardInstance* ins, CanardRxTransfer* transfer)
+{
+    dronecan_remoteid_SecureCommandRequest req;
+    if (dronecan_remoteid_SecureCommandRequest_decode(transfer, &req)) {
+        return;
+    }
+
+    dronecan_remoteid_SecureCommandResponse reply {};
+    reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_UNSUPPORTED;
+    reply.sequence = req.sequence;
+    reply.operation = req.operation;
+
+    if (!check_signature(req)) {
+        reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_DENIED;
+        goto send_reply;
+    }
+    
+    switch (req.operation) {
+    case DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_GET_REMOTEID_SESSION_KEY: {
+        make_session_key(session_key);
+        memcpy(reply.data.data, session_key, sizeof(session_key));
+        reply.data.len = sizeof(session_key);
+        reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_ACCEPTED;
+        break;
+    }
+    case DRONECAN_REMOTEID_SECURECOMMAND_REQUEST_SECURE_COMMAND_SET_REMOTEID_CONFIG: {
+        Serial.printf("SECURE_COMMAND_SET_REMOTEID_CONFIG\n");
+        int16_t data_len = req.data.len - req.sig_length;
+        req.data.data[data_len] = 0;
+        /*
+          command buffer is nul separated set of NAME=VALUE pairs
+         */
+        reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_ACCEPTED;
+        char *command = (char *)req.data.data;
+        while (data_len > 0) {
+            uint8_t cmdlen = strlen(command);
+            Serial.printf("set_config %s", command);
+            char *eq = strchr(command, '=');
+            if (eq != nullptr) {
+                *eq = 0;
+                if (!g.set_by_name_string(command, eq+1)) {
+                    reply.result = DRONECAN_REMOTEID_SECURECOMMAND_RESPONSE_RESULT_FAILED;
+                }
+            }
+            command += cmdlen+1;
+            data_len -= cmdlen+1;
+        }
+        break;
+    }
+    }
+
+send_reply:
+    uint8_t buffer[UAVCAN_PROTOCOL_PARAM_GETSET_RESPONSE_MAX_SIZE] {};
+    uint16_t total_size = dronecan_remoteid_SecureCommandResponse_encode(&reply, buffer);
+
+    canardRequestOrRespond(ins,
+                           transfer->source_node_id,
+                           DRONECAN_REMOTEID_SECURECOMMAND_SIGNATURE,
+                           DRONECAN_REMOTEID_SECURECOMMAND_ID,
+                           &transfer->transfer_id,
+                           transfer->priority,
+                           CanardResponse,
+                           &buffer[0],
+                           total_size);
+}
+
 // printf to CAN LogMessage for debugging
 void DroneCAN::can_printf(const char *fmt, ...)
 {

RemoteIDModule/DroneCAN.h

@@ -10,9 +10,11 @@
 #include <dronecan.remoteid.SelfID.h>
 #include <dronecan.remoteid.System.h>
 #include <dronecan.remoteid.OperatorID.h>
+#include <dronecan.remoteid.SecureCommand.h>
 
 #define CAN_POOL_SIZE 4096
 
+
 class DroneCAN : public Transport {
 public:
     using Transport::Transport;
@@ -47,6 +49,7 @@ 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;
 
@@ -56,8 +59,11 @@ private:
     void handle_System(CanardRxTransfer* transfer);
     void handle_Location(CanardRxTransfer* transfer);
     void handle_param_getset(CanardInstance* ins, CanardRxTransfer* transfer);
+    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/check_firmware.cpp

@@ -3,50 +3,7 @@
 #include "monocypher.h"
 #include "parameters.h"
 #include <string.h>
-
-/*
-  simple base64 decoder, not particularly efficient, but small
- */
-static 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;
-    while (*s && (p=strchr(b64,*s))) {
-        const uint8_t idx = (p - b64);
-        const uint32_t byte_offset = (i*6)/8;
-        const uint32_t bit_offset = (i*6)%8;
-        out[byte_offset] &= ~((1<<(8-bit_offset))-1);
-        if (bit_offset < 3) {
-            if (byte_offset >= max_len) {
-                break;
-            }
-            out[byte_offset] |= (idx << (2-bit_offset));
-            n = byte_offset+1;
-        } else {
-            if (byte_offset >= max_len) {
-                break;
-            }
-            out[byte_offset] |= (idx >> (bit_offset-2));
-            n = byte_offset+1;
-            if (byte_offset+1 >= max_len) {
-                break;
-            }
-            out[byte_offset+1] = (idx << (8-(bit_offset-2))) & 0xFF;
-            n = byte_offset+2;
-        }
-        s++; i++;
-    }
-
-    if ((n > 0) && (*s == '=')) {
-        n -= 1;
-    }
-
-    return n;
-}
-
+#include "util.h"
 
 bool CheckFirmware::check_partition(const uint8_t *flash, uint32_t flash_len,
                                     const uint8_t *lead_bytes, uint32_t lead_length,
@@ -92,20 +49,15 @@ bool CheckFirmware::check_OTA_partition(const esp_partition_t *part, const uint8
     }
     board_id = ad->board_id;
 
-    bool no_keys = true;
+    if (g.no_public_keys()) {
+        Serial.printf("No public keys - accepting firmware\n");
+        spi_flash_munmap(handle);
+        return true;
+    }
 
     for (uint8_t i=0; i<MAX_PUBLIC_KEYS; i++) {
-        const char *b64_key = g.public_keys[i].b64_key;
-        Serial.printf("Checking public key: '%s'\n", b64_key);
-        const char *ktype = "PUBLIC_KEYV1:";
-        if (strncmp(b64_key, ktype, strlen(ktype)) != 0) {
-            continue;
-        }
-        no_keys = false;
-        b64_key += strlen(ktype);
         uint8_t key[32];
-        int32_t out_len = base64_decode(b64_key, key, sizeof(key));
-        if (out_len != 32) {
+        if (!g.get_public_key(i, key)) {
             continue;
         }
         if (check_partition((const uint8_t *)ptr, img_len, lead_bytes, lead_length, ad, key)) {
@@ -116,10 +68,6 @@ bool CheckFirmware::check_OTA_partition(const esp_partition_t *part, const uint8
         Serial.printf("check failed key %u\n", i);
     }
     spi_flash_munmap(handle);
-    if (no_keys) {
-        Serial.printf("No public keys - accepting firmware\n");
-        return true;
-    }
     Serial.printf("firmware failed checks\n");
     return false;
 }

RemoteIDModule/parameters.cpp

@@ -4,6 +4,7 @@
 #include <nvs_flash.h>
 #include <string.h>
 #include "romfs.h"
+#include "util.h"
 
 Parameters g;
 static nvs_handle handle;
@@ -232,3 +233,61 @@ bool Parameters::set_by_name_char64(const char *name, const char *s)
     f->set_char64(s);
     return true;
 }
+
+bool Parameters::set_by_name_string(const char *name, const char *s)
+{
+    const auto *f = find(name);
+    if (!f) {
+        return false;
+    }
+    switch (f->ptype) {
+        case ParamType::UINT8:
+            f->set_uint8(uint8_t(strtoul(s, nullptr, 0)));
+            return true;
+        case ParamType::UINT32:
+            f->set_uint32(strtoul(s, nullptr, 0));
+            return true;
+        case ParamType::FLOAT:
+            f->set_float(atof(s));
+            return true;
+        case ParamType::CHAR20:
+            f->set_char20(s);
+            return true;
+        case ParamType::CHAR64:
+            f->set_char64(s);
+            return true;
+    }
+    return false;
+}
+
+/*
+  return a public key
+ */
+bool Parameters::get_public_key(uint8_t i, uint8_t key[32]) const
+{
+    if (i >= MAX_PUBLIC_KEYS) {
+        return false;
+    }
+    const char *b64_key = g.public_keys[i].b64_key;
+    const char *ktype = "PUBLIC_KEYV1:";
+    if (strncmp(b64_key, ktype, strlen(ktype)) != 0) {
+        return false;
+    }
+    b64_key += strlen(ktype);
+    int32_t out_len = base64_decode(b64_key, key, 32);
+    if (out_len != 32) {
+        return false;
+    }
+    return true;
+}
+
+bool Parameters::no_public_keys(void) const
+{
+    for (uint8_t i=0; i<MAX_PUBLIC_KEYS; i++) {
+        uint8_t key[32];
+        if (get_public_key(i, key)) {
+            return false;
+        }
+    }
+    return true;
+}

RemoteIDModule/parameters.h

@@ -73,6 +73,14 @@ public:
 
     bool set_by_name_uint8(const char *name, uint8_t v);
     bool set_by_name_char64(const char *name, const char *s);
+    bool set_by_name_string(const char *name, const char *s);
+
+    /*
+      return a public key
+    */
+    bool get_public_key(uint8_t i, uint8_t key[32]) const;
+    bool no_public_keys(void) const;
+
 private:
     void load_defaults(void);
 };

RemoteIDModule/util.cpp

@@ -0,0 +1,73 @@
+#include "util.h"
+#include <string.h>
+
+/*
+  64 bit crc from ArduPilot
+*/
+uint64_t crc_crc64(const uint32_t *data, uint16_t num_words)
+{
+    const uint64_t poly = 0x42F0E1EBA9EA3693ULL;
+    uint64_t crc = ~(0ULL);
+    while (num_words--) {
+        uint32_t value = *data++;
+        for (uint8_t j = 0; j < 4; j++) {
+            uint8_t byte = ((uint8_t *)&value)[j];
+            crc ^= (uint64_t)byte << 56u;
+            for (uint8_t i = 0; i < 8; i++) {
+                if (crc & (1ull << 63u)) {
+                    crc = (uint64_t)(crc << 1u) ^ poly;
+                } else {
+                    crc = (uint64_t)(crc << 1u);
+                }
+            }
+        }
+    }
+    crc ^= ~(0ULL);
+
+    return crc;
+}
+
+/*
+  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;
+    while (*s && (p=strchr(b64,*s))) {
+        const uint8_t idx = (p - b64);
+        const uint32_t byte_offset = (i*6)/8;
+        const uint32_t bit_offset = (i*6)%8;
+        out[byte_offset] &= ~((1<<(8-bit_offset))-1);
+        if (bit_offset < 3) {
+            if (byte_offset >= max_len) {
+                break;
+            }
+            out[byte_offset] |= (idx << (2-bit_offset));
+            n = byte_offset+1;
+        } else {
+            if (byte_offset >= max_len) {
+                break;
+            }
+            out[byte_offset] |= (idx >> (bit_offset-2));
+            n = byte_offset+1;
+            if (byte_offset+1 >= max_len) {
+                break;
+            }
+            out[byte_offset+1] = (idx << (8-(bit_offset-2))) & 0xFF;
+            n = byte_offset+2;
+        }
+        s++; i++;
+    }
+
+    if ((n > 0) && (*s == '=')) {
+        n -= 1;
+    }
+
+    return n;
+}
+
+

RemoteIDModule/util.h

@@ -0,0 +1,17 @@
+#pragma once
+
+#ifndef MIN
+#define MIN(a,b) ((a)<(b)?(a):(b))
+#endif
+
+#include <stdint.h>
+
+/*
+  64 bit crc from ArduPilot
+*/
+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);

scripts/secure_command.py

@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+'''
+perform a secure parameter change on a ArduRemoteID node via DroneCAN
+user must supply a private key corresponding to one of the public keys on the node
+'''
+
+import dronecan, time, sys, random, base64, struct
+from dronecan import uavcan
+
+try:
+    import monocypher
+except ImportError:
+    print("Please install monocypher with: python3 -m pip install pymonocypher")
+    sys.exit(1)
+
+
+# get command line arguments
+from argparse import ArgumentParser
+parser = ArgumentParser(description='secure_command')
+parser.add_argument("--bitrate", default=1000000, type=int, help="CAN bit rate")
+parser.add_argument("--node-id", default=100, type=int, help="local CAN node ID")
+parser.add_argument("--target-node", default=None, type=int, help="target node ID")
+parser.add_argument("--private-key", default=None, type=str, help="private key file")
+parser.add_argument("uri", default=None, type=str, help="CAN URI")
+parser.add_argument("paramop", default=None, type=str, help="parameter operation")
+args = parser.parse_args()
+
+should_exit = False
+
+if args.target_node is None:
+    print("Must specify target node ID")
+    should_exit = True
+
+if args.private_key is None:
+    print("Must specify private key file")
+    should_exit = True
+
+if should_exit:
+    sys.exit(1)
+
+SECURE_COMMAND_GET_REMOTEID_SESSION_KEY = dronecan.dronecan.remoteid.SecureCommand.Request().SECURE_COMMAND_GET_REMOTEID_SESSION_KEY
+SECURE_COMMAND_SET_REMOTEID_CONFIG = dronecan.dronecan.remoteid.SecureCommand.Request().SECURE_COMMAND_SET_REMOTEID_CONFIG
+session_key = None
+sequence = random.randint(0, 0xFFFFFFFF)
+last_session_key_req = 0
+last_set_config = 0
+
+# Initializing a DroneCAN node instance.
+node = dronecan.make_node(args.uri, node_id=args.node_id, bitrate=args.bitrate)
+
+# Initializing a node monitor
+node_monitor = dronecan.app.node_monitor.NodeMonitor(node)
+
+def get_session_key_response(reply):
+    if not reply:
+        # timed out
+        return
+    global session_key
+    session_key = bytearray(reply.response.data)
+    print("Got session key")
+
+def get_private_key():
+    '''get private key, return 32 byte key or None'''
+    if args.private_key is None:
+        return None
+    try:
+        d = open(args.private_key,'r').read()
+    except Exception as ex:
+        return None
+    ktype = "PRIVATE_KEYV1:"
+    if not d.startswith(ktype):
+        return None
+    return base64.b64decode(d[len(ktype):])
+    
+def make_signature(seq, command, data):
+    '''make a signature'''
+    private_key = get_private_key()
+    d = struct.pack("<II", seq, command)
+    d += data
+    if command != SECURE_COMMAND_GET_REMOTEID_SESSION_KEY:
+        if session_key is None:
+            print("No session key")
+            raise Exception("No session key")
+        d += session_key
+    return monocypher.signature_sign(private_key, d)
+
+def request_session_key():
+    '''request a session key'''
+    global sequence
+    sig = make_signature(sequence, SECURE_COMMAND_GET_REMOTEID_SESSION_KEY, bytes())
+    node.request(dronecan.dronecan.remoteid.SecureCommand.Request(
+            sequence=sequence,
+            operation=SECURE_COMMAND_GET_REMOTEID_SESSION_KEY,
+            sig_length=len(sig),
+            data=sig),
+        args.target_node,
+        get_session_key_response)
+    sequence = (sequence+1) % (1<<32)
+    print("Requested session key")
+
+def config_change_response(reply):
+    if not reply:
+        # timed out
+        return
+    result_map = {
+        0: "ACCEPTED",
+        1: "TEMPORARILY_REJECTED",
+        2: "DENIED",
+        3: "UNSUPPORTED",
+        4: "FAILED" }
+    result = result_map.get(reply.response.result, "invalid")
+    print("Got change response: %s" % result)
+    sys.exit(reply.response.result)
+    
+def send_config_change():
+    '''send remoteid config change'''
+    global sequence
+    req = args.paramop.encode('utf-8')
+    sig = make_signature(sequence, SECURE_COMMAND_SET_REMOTEID_CONFIG, req)
+    node.request(dronecan.dronecan.remoteid.SecureCommand.Request(
+            sequence=sequence,
+            operation=SECURE_COMMAND_SET_REMOTEID_CONFIG,
+            sig_length=len(sig),
+            data=req+sig),
+        args.target_node,
+        config_change_response)
+    sequence = (sequence+1) % (1<<32)
+    print("Requested config change")
+    
+def update():
+    now = time.time()
+    global last_session_key_req, last_set_config, session_key
+    if session_key is None and now - last_session_key_req > 2.0:
+        last_session_key_req = now
+        request_session_key()
+    if session_key is not None and now - last_set_config > 2.0:
+        last_set_config = now
+        send_config_change()
+
+while True:
+    try:
+        update()
+        node.spin(timeout=0.1)
+    except Exception as ex:
+        print(ex)