mirror of https://github.com/ArduPilot/ardupilot
AP_CheckFirmware: support updating or removing public keys
this supports fetching, updating and removing public keys using the SECURE_COMMAND mavlink operations. This will allow for vendors to remotely hand over management of RemoteID security to new vendors
This commit is contained in:
Andrew Tridgell
parent
611f26f16c
commit
1f4db8204b
|
|
@ -14,17 +14,7 @@
|
|||
#include <string.h>
|
||||
#include "monocypher.h"
|
||||
|
||||
#define AP_PUBLIC_KEY_LEN 32
|
||||
#define AP_PUBLIC_KEY_MAX_KEYS 10
|
||||
|
||||
struct PACKED secure_data {
|
||||
uint8_t sig[8] = {0x4e, 0xcf, 0x4e, 0xa5, 0xa6, 0xb6, 0xf7, 0x29};
|
||||
struct PACKED {
|
||||
uint8_t key[AP_PUBLIC_KEY_LEN] = {};
|
||||
} public_key[AP_PUBLIC_KEY_MAX_KEYS];
|
||||
};
|
||||
|
||||
const struct secure_data public_keys __attribute__((section(".ecc_raw")));
|
||||
const struct ap_secure_data public_keys __attribute__((section(".ecc_raw")));
|
||||
|
||||
/*
|
||||
check a signature against bootloader keys
|
||||
|
|
@ -156,4 +146,5 @@ void check_firmware_print(void)
|
|||
}
|
||||
#endif
|
||||
|
||||
|
||||
#endif // AP_CHECK_FIRMWARE_ENABLED
|
||||
|
|
|
|||
|
|
@ -6,6 +6,9 @@
|
|||
#include <AP_HAL/AP_HAL_Boards.h>
|
||||
#include <AP_OpenDroneID/AP_OpenDroneID_config.h>
|
||||
#include <AP_HAL/AP_HAL.h>
|
||||
#ifndef HAL_BOOTLOADER_BUILD
|
||||
#include <GCS_MAVLink/GCS.h>
|
||||
#endif
|
||||
|
||||
#ifndef AP_CHECK_FIRMWARE_ENABLED
|
||||
#define AP_CHECK_FIRMWARE_ENABLED AP_OPENDRONEID_ENABLED
|
||||
|
|
@ -82,9 +85,61 @@ struct app_descriptor {
|
|||
|
||||
static_assert(sizeof(app_descriptor) == APP_DESCRIPTOR_TOTAL_LENGTH, "app_descriptor incorrect length");
|
||||
|
||||
#if AP_SIGNED_FIRMWARE
|
||||
|
||||
#define AP_PUBLIC_KEY_LEN 32
|
||||
#define AP_PUBLIC_KEY_MAX_KEYS 10
|
||||
#define AP_PUBLIC_KEY_SIGNATURE {0x4e, 0xcf, 0x4e, 0xa5, 0xa6, 0xb6, 0xf7, 0x29}
|
||||
|
||||
struct PACKED ap_secure_data {
|
||||
uint8_t sig[8] = AP_PUBLIC_KEY_SIGNATURE;
|
||||
struct PACKED {
|
||||
uint8_t key[AP_PUBLIC_KEY_LEN] = {};
|
||||
} public_key[AP_PUBLIC_KEY_MAX_KEYS];
|
||||
};
|
||||
#endif
|
||||
|
||||
#ifdef HAL_BOOTLOADER_BUILD
|
||||
check_fw_result_t check_good_firmware(void);
|
||||
#endif
|
||||
#else
|
||||
void check_firmware_print(void);
|
||||
|
||||
#ifdef HAL_GCS_ENABLED
|
||||
class AP_CheckFirmware {
|
||||
public:
|
||||
// handle a message from the GCS. This is static as we don't have an AP_CheckFirmware object
|
||||
static void handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg);
|
||||
static void handle_secure_command(mavlink_channel_t chan, const mavlink_secure_command_t &pkt);
|
||||
static bool check_signature(const mavlink_secure_command_t &pkt);
|
||||
static const struct ap_secure_data *find_public_keys(void);
|
||||
|
||||
/*
|
||||
in memory structure representing the current bootloader. It has two
|
||||
data regions to cope with persistent data at the end of the
|
||||
bootloader sector
|
||||
*/
|
||||
struct bl_data {
|
||||
uint32_t length1;
|
||||
uint8_t *data1;
|
||||
uint32_t offset2;
|
||||
uint32_t length2;
|
||||
uint8_t *data2;
|
||||
|
||||
// destructor
|
||||
~bl_data(void) {
|
||||
delete[] data1;
|
||||
delete[] data2;
|
||||
}
|
||||
};
|
||||
static struct bl_data *read_bootloader(void);
|
||||
static bool write_bootloader(const struct bl_data *bld);
|
||||
static bool set_public_keys(uint8_t key_idx, uint8_t num_keys, const uint8_t *key_data);
|
||||
|
||||
private:
|
||||
static uint8_t session_key[8];
|
||||
};
|
||||
#endif
|
||||
|
||||
#endif // HAL_BOOTLOADER_BUILD
|
||||
|
||||
#endif // AP_CHECK_FIRMWARE_ENABLED
|
||||
|
|
|
|||
|
|
@ -0,0 +1,383 @@
|
|||
/*
|
||||
support checking board ID and firmware CRC in the bootloader
|
||||
*/
|
||||
#include "AP_CheckFirmware.h"
|
||||
#include <AP_HAL/HAL.h>
|
||||
|
||||
#if AP_CHECK_FIRMWARE_ENABLED && AP_SIGNED_FIRMWARE && !defined(HAL_BOOTLOADER_BUILD)
|
||||
|
||||
#include "monocypher.h"
|
||||
#include <AP_Math/crc.h>
|
||||
|
||||
extern const AP_HAL::HAL &hal;
|
||||
|
||||
uint8_t AP_CheckFirmware::session_key[8];
|
||||
|
||||
/*
|
||||
make a session key
|
||||
*/
|
||||
static void make_session_key(uint8_t key[8])
|
||||
{
|
||||
struct {
|
||||
uint32_t time_us;
|
||||
uint8_t unique_id[12];
|
||||
uint16_t rand1;
|
||||
uint16_t rand2;
|
||||
} data {};
|
||||
static_assert(sizeof(data) % 4 == 0, "data must be multiple of 4 bytes");
|
||||
|
||||
// get data which will not apply on a different board, and includes some randomness
|
||||
uint8_t uid_len = 12;
|
||||
hal.util->get_system_id_unformatted(data.unique_id, uid_len);
|
||||
data.time_us = AP_HAL::micros();
|
||||
data.rand1 = get_random16();
|
||||
data.rand2 = get_random16();
|
||||
const uint64_t c64 = crc_crc64((const uint32_t *)&data, sizeof(data)/sizeof(uint32_t));
|
||||
memcpy(key, (uint8_t *)&c64, 8);
|
||||
}
|
||||
|
||||
/*
|
||||
find public keys in bootloader, or return NULL if signature not found
|
||||
|
||||
this assumes the public keys are in the first sector
|
||||
*/
|
||||
const struct ap_secure_data *AP_CheckFirmware::find_public_keys(void)
|
||||
{
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
const uint32_t page_size = hal.flash->getpagesize(0);
|
||||
const uint32_t flash_addr = hal.flash->getpageaddr(0);
|
||||
const uint8_t *flash = (const uint8_t *)flash_addr;
|
||||
const uint8_t key[] = AP_PUBLIC_KEY_SIGNATURE;
|
||||
return (const struct ap_secure_data *)memmem(flash, page_size, key, sizeof(key));
|
||||
#else
|
||||
return nullptr;
|
||||
#endif
|
||||
}
|
||||
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
/*
|
||||
return true if 1k of data is all 0xff (empty flash)
|
||||
*/
|
||||
static bool empty_1k(const uint8_t *data)
|
||||
{
|
||||
for (uint32_t i=0; i<1024; i++) {
|
||||
if (data[i] != 0xFFU) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
return true;
|
||||
}
|
||||
#endif
|
||||
|
||||
/*
|
||||
read bootloader into memory. This is complicated by the potential presence
|
||||
of persistent data from temperature calibration at the end of the sector
|
||||
|
||||
Also note this assumes the public keys are in the first sector if
|
||||
the bootloader covers more than one sector. This is a reasonable
|
||||
assumption given the linker file
|
||||
*/
|
||||
AP_CheckFirmware::bl_data *AP_CheckFirmware::read_bootloader(void)
|
||||
{
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
struct bl_data *bld = new bl_data;
|
||||
if (bld == nullptr) {
|
||||
return nullptr;
|
||||
}
|
||||
const uint32_t page_size = hal.flash->getpagesize(0);
|
||||
const uint32_t flash_addr = hal.flash->getpageaddr(0);
|
||||
const uint8_t *flash = (uint8_t *)flash_addr;
|
||||
const uint16_t block_size = 1024;
|
||||
uint16_t num_blocks = page_size / block_size;
|
||||
/*
|
||||
find first empty block
|
||||
*/
|
||||
for (uint16_t i=0; i<num_blocks; i++) {
|
||||
if (empty_1k(&flash[block_size*i])) {
|
||||
break;
|
||||
}
|
||||
bld->length1 += block_size;
|
||||
}
|
||||
bld->data1 = new uint8_t[bld->length1];
|
||||
if (bld->data1 == nullptr) {
|
||||
delete bld;
|
||||
return nullptr;
|
||||
}
|
||||
memcpy(bld->data1, flash, bld->length1);
|
||||
flash += bld->length1;
|
||||
num_blocks -= bld->length1 / block_size;
|
||||
|
||||
/*
|
||||
find first non-empty block, which should be the persistent data if-any
|
||||
*/
|
||||
bld->offset2 = bld->length1;
|
||||
while (num_blocks > 0) {
|
||||
if (!empty_1k(&flash[bld->offset2])) {
|
||||
break;
|
||||
}
|
||||
num_blocks--;
|
||||
bld->offset2 += block_size;
|
||||
}
|
||||
if (num_blocks > 0) {
|
||||
// we have persistent data to save
|
||||
bld->length2 = num_blocks * block_size;
|
||||
bld->data2 = new uint8_t[bld->length2];
|
||||
if (bld->data2 == nullptr) {
|
||||
delete bld;
|
||||
return nullptr;
|
||||
}
|
||||
memcpy(bld->data2, &flash[bld->offset2], bld->length2);
|
||||
}
|
||||
return bld;
|
||||
#else
|
||||
return nullptr;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
write bootloader from memory
|
||||
*/
|
||||
bool AP_CheckFirmware::write_bootloader(const struct bl_data *bld)
|
||||
{
|
||||
#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
|
||||
const uint32_t flash_addr = hal.flash->getpageaddr(0);
|
||||
EXPECT_DELAY_MS(3000);
|
||||
if (!hal.flash->erasepage(0)) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Bootloader erase failed");
|
||||
return false;
|
||||
}
|
||||
EXPECT_DELAY_MS(3000);
|
||||
if (!hal.flash->write(flash_addr, bld->data1, bld->length1)) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Bootloader write1 failed");
|
||||
return false;
|
||||
}
|
||||
EXPECT_DELAY_MS(3000);
|
||||
if (bld->length2 != 0 &&
|
||||
!hal.flash->write(flash_addr+bld->offset2, bld->data2, bld->length2)) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Bootloader write1 failed");
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
#else
|
||||
return false;
|
||||
#endif
|
||||
}
|
||||
|
||||
/*
|
||||
check signature in a command against bootloader public keys
|
||||
*/
|
||||
bool AP_CheckFirmware::check_signature(const mavlink_secure_command_t &pkt)
|
||||
{
|
||||
const struct ap_secure_data *sec_data = find_public_keys();
|
||||
if (sec_data == nullptr) {
|
||||
return false;
|
||||
}
|
||||
if (pkt.sig_length != 64) {
|
||||
// monocypher signatures are 64 bytes
|
||||
return false;
|
||||
}
|
||||
const uint8_t zero_key[AP_PUBLIC_KEY_LEN] {};
|
||||
bool all_zero_keys = true;
|
||||
/*
|
||||
look over all public keys, if one matches then we are OK
|
||||
*/
|
||||
for (const auto &public_key : sec_data->public_key) {
|
||||
if (memcmp(public_key.key, zero_key, AP_PUBLIC_KEY_LEN) == 0) {
|
||||
continue;
|
||||
}
|
||||
all_zero_keys = false;
|
||||
|
||||
crypto_check_ctx ctx {};
|
||||
crypto_check_ctx_abstract *actx = (crypto_check_ctx_abstract*)&ctx;
|
||||
crypto_check_init(actx, &pkt.data[pkt.data_length], public_key.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, pkt.data_length);
|
||||
if (pkt.operation != SECURE_COMMAND_GET_SESSION_KEY) {
|
||||
crypto_check_update(actx, session_key, sizeof(session_key));
|
||||
}
|
||||
if (crypto_check_final(actx) == 0) {
|
||||
// good signature
|
||||
return true;
|
||||
}
|
||||
}
|
||||
return all_zero_keys;
|
||||
}
|
||||
|
||||
/*
|
||||
set public keys in bootloader
|
||||
*/
|
||||
bool AP_CheckFirmware::set_public_keys(uint8_t key_idx, uint8_t num_keys, const uint8_t *key_data)
|
||||
{
|
||||
auto *bld = read_bootloader();
|
||||
if (bld == nullptr) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Failed to load bootloader into memory");
|
||||
return false;
|
||||
}
|
||||
const uint8_t key[] = AP_PUBLIC_KEY_SIGNATURE;
|
||||
struct ap_secure_data *sec_data = (struct ap_secure_data *)memmem(bld->data1, bld->length1, key, sizeof(key));
|
||||
if (sec_data == nullptr) {
|
||||
delete bld;
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_INFO, "Failed to find key signature");
|
||||
return false;
|
||||
}
|
||||
memcpy(sec_data->public_key[key_idx].key, key_data, num_keys*AP_PUBLIC_KEY_LEN);
|
||||
|
||||
/*
|
||||
pack so non-zero keys are at the start
|
||||
*/
|
||||
const uint8_t zero_key[AP_PUBLIC_KEY_LEN] {};
|
||||
uint8_t max_keys = AP_PUBLIC_KEY_MAX_KEYS;
|
||||
for (uint8_t i=0; max_keys>1 && i<max_keys-1; i++) {
|
||||
if (memcmp(zero_key, sec_data->public_key[i].key, AP_PUBLIC_KEY_LEN) == 0) {
|
||||
memmove(sec_data->public_key[i].key, sec_data->public_key[i+1].key, AP_PUBLIC_KEY_LEN*(max_keys-(i+1)));
|
||||
max_keys--;
|
||||
i--;
|
||||
}
|
||||
}
|
||||
memset(sec_data->public_key[max_keys-1].key, 0, AP_PUBLIC_KEY_LEN*(AP_PUBLIC_KEY_MAX_KEYS-max_keys));
|
||||
|
||||
bool ret = write_bootloader(bld);
|
||||
delete bld;
|
||||
return ret;
|
||||
}
|
||||
|
||||
/*
|
||||
handle a SECURE_COMMAND
|
||||
*/
|
||||
void AP_CheckFirmware::handle_secure_command(mavlink_channel_t chan, 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)) {
|
||||
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: {
|
||||
const struct ap_secure_data *sec_data = find_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) / AP_PUBLIC_KEY_LEN;
|
||||
if (key_idx >= AP_PUBLIC_KEY_MAX_KEYS ||
|
||||
num_keys > max_fetch ||
|
||||
key_idx+num_keys > AP_PUBLIC_KEY_MAX_KEYS ||
|
||||
sec_data == nullptr) {
|
||||
reply.result = MAV_RESULT_FAILED;
|
||||
goto send_reply;
|
||||
}
|
||||
|
||||
// remove zero keys
|
||||
const uint8_t zero_key[AP_PUBLIC_KEY_LEN] {};
|
||||
while (num_keys > 0 &&
|
||||
memcmp(zero_key, &sec_data->public_key[key_idx+num_keys-1], AP_PUBLIC_KEY_LEN) == 0) {
|
||||
num_keys--;
|
||||
}
|
||||
|
||||
reply.data_length = 1+num_keys*AP_PUBLIC_KEY_LEN;
|
||||
reply.data[0] = key_idx;
|
||||
memcpy(&reply.data[1], &sec_data->public_key[key_idx], reply.data_length-1);
|
||||
reply.result = MAV_RESULT_ACCEPTED;
|
||||
break;
|
||||
}
|
||||
|
||||
case SECURE_COMMAND_SET_PUBLIC_KEYS: {
|
||||
if (pkt.data_length < AP_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) / AP_PUBLIC_KEY_LEN;
|
||||
if (num_keys == 0) {
|
||||
reply.result = MAV_RESULT_FAILED;
|
||||
goto send_reply;
|
||||
}
|
||||
if (key_idx >= AP_PUBLIC_KEY_MAX_KEYS ||
|
||||
key_idx+num_keys > AP_PUBLIC_KEY_MAX_KEYS) {
|
||||
reply.result = MAV_RESULT_FAILED;
|
||||
goto send_reply;
|
||||
}
|
||||
if (set_public_keys(key_idx, num_keys, &pkt.data[1])) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Bootloader update OK");
|
||||
reply.result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
reply.result = MAV_RESULT_FAILED;
|
||||
}
|
||||
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 >= AP_PUBLIC_KEY_MAX_KEYS ||
|
||||
key_idx+num_keys > AP_PUBLIC_KEY_MAX_KEYS) {
|
||||
reply.result = MAV_RESULT_FAILED;
|
||||
goto send_reply;
|
||||
}
|
||||
uint8_t *data = new uint8_t[num_keys*AP_PUBLIC_KEY_LEN];
|
||||
if (data == nullptr) {
|
||||
reply.result = MAV_RESULT_FAILED;
|
||||
goto send_reply;
|
||||
}
|
||||
if (set_public_keys(key_idx, num_keys, data)) {
|
||||
GCS_SEND_TEXT(MAV_SEVERITY_ERROR, "Bootloader update OK");
|
||||
reply.result = MAV_RESULT_ACCEPTED;
|
||||
} else {
|
||||
reply.result = MAV_RESULT_FAILED;
|
||||
}
|
||||
delete[] data;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
send_reply:
|
||||
// send reply
|
||||
mavlink_msg_secure_command_reply_send_struct(chan, &reply);
|
||||
}
|
||||
|
||||
/*
|
||||
implement secure command operations for updating public keys
|
||||
*/
|
||||
void AP_CheckFirmware::handle_msg(mavlink_channel_t chan, const mavlink_message_t &msg)
|
||||
{
|
||||
switch (msg.msgid) {
|
||||
case MAVLINK_MSG_ID_SECURE_COMMAND: {
|
||||
mavlink_secure_command_t pkt;
|
||||
mavlink_msg_secure_command_decode(&msg, &pkt);
|
||||
handle_secure_command(chan, pkt);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
#endif // AP_CHECK_FIRMWARE_ENABLED
|
||||
Loading…
Reference in New Issue