AP_CheckFirmware: added library for checking firmware ID and CRC

libraries/AP_CheckFirmware/AP_CheckFirmware.cpp

@@ -0,0 +1,78 @@
+/*
+  support checking board ID and firmware CRC in the bootloader
+ */
+#include "AP_CheckFirmware.h"
+#include <AP_HAL/HAL.h>
+#include <AP_Math/crc.h>
+
+#if AP_CHECK_FIRMWARE_ENABLED
+
+#if defined(HAL_BOOTLOADER_BUILD)
+/*
+  check firmware CRC and board ID to see if it matches
+ */
+check_fw_result_t check_good_firmware(void)
+{
+    const uint8_t sig[8] = { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 };
+    const uint8_t *flash = (const uint8_t *)(FLASH_LOAD_ADDRESS + (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB)*1024);
+    const uint32_t flash_size = (BOARD_FLASH_SIZE - (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB))*1024;
+    const app_descriptor *ad = (const app_descriptor *)memmem(flash, flash_size-sizeof(app_descriptor), sig, sizeof(sig));
+    if (ad == nullptr) {
+        // no application signature
+        return check_fw_result_t::FAIL_REASON_NO_APP_SIG;
+    }
+    // check length
+    if (ad->image_size > flash_size) {
+        return check_fw_result_t::FAIL_REASON_BAD_LENGTH_APP;
+    }
+
+    bool id_ok = (ad->board_id == APJ_BOARD_ID);
+#ifdef ALT_BOARD_ID
+    id_ok |= (ad->board_id == ALT_BOARD_ID);
+#endif
+
+    if (!id_ok) {
+        return check_fw_result_t::FAIL_REASON_BAD_BOARD_ID;
+    }
+
+    const uint8_t desc_len = offsetof(app_descriptor, version_major) - offsetof(app_descriptor, image_crc1);
+    uint32_t len1 = ((const uint8_t *)&ad->image_crc1) - flash;
+    if ((len1 + desc_len) > ad->image_size) {
+        return check_fw_result_t::FAIL_REASON_BAD_LENGTH_DESCRIPTOR;
+    }
+
+    uint32_t len2 = ad->image_size - (len1 + desc_len);
+    uint32_t crc1 = crc32_small(0, flash, len1);
+    uint32_t crc2 = crc32_small(0, (const uint8_t *)&ad->version_major, len2);
+    if (crc1 != ad->image_crc1 || crc2 != ad->image_crc2) {
+        return check_fw_result_t::FAIL_REASON_BAD_CRC;
+    }
+    return check_fw_result_t::CHECK_FW_OK;
+}
+#endif // HAL_BOOTLOADER_BUILD
+
+#if !defined(HAL_BOOTLOADER_BUILD)
+extern const AP_HAL::HAL &hal;
+
+/*
+  declare constant app_descriptor in flash
+ */
+extern const struct app_descriptor app_descriptor;
+#if CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+const struct app_descriptor app_descriptor __attribute__((section(".app_descriptor")));
+#else
+const struct app_descriptor app_descriptor;
+#endif
+
+/*
+  this is needed to ensure we don't elide the app_descriptor
+ */
+void check_firmware_print(void)
+{
+    hal.console->printf("Booting %u/%u\n",
+                        app_descriptor.version_major,
+                        app_descriptor.version_minor);
+}
+#endif
+
+#endif // AP_CHECK_FIRMWARE_ENABLED

libraries/AP_CheckFirmware/AP_CheckFirmware.h

@@ -0,0 +1,71 @@
+/*
+  support checking board ID and firmware CRC in the bootloader
+ */
+#pragma once
+
+#include <AP_HAL/AP_HAL_Boards.h>
+#include <AP_OpenDroneID/AP_OpenDroneID_config.h>
+
+#ifndef AP_CHECK_FIRMWARE_ENABLED
+#define AP_CHECK_FIRMWARE_ENABLED AP_OPENDRONEID_ENABLED
+#endif
+
+#if AP_CHECK_FIRMWARE_ENABLED
+
+enum class check_fw_result_t : uint8_t {
+    CHECK_FW_OK = 0,
+    FAIL_REASON_NO_APP_SIG = 10,
+    FAIL_REASON_BAD_LENGTH_APP = 11,
+    FAIL_REASON_BAD_BOARD_ID = 12,
+    FAIL_REASON_BAD_CRC = 13,
+    FAIL_REASON_IN_UPDATE = 14,
+    FAIL_REASON_WATCHDOG = 15,
+    FAIL_REASON_BAD_LENGTH_DESCRIPTOR = 16,
+};
+
+#ifndef FW_MAJOR
+#define APP_FW_MAJOR 0
+#define APP_FW_MINOR 0
+#else
+#define APP_FW_MAJOR FW_MAJOR
+#define APP_FW_MINOR FW_MINOR
+#endif
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL && !defined(APJ_BOARD_ID)
+// this allows for sitl_periph_gps to build
+#define APJ_BOARD_ID 0
+#endif
+
+/*
+ the app_descriptor stored in flash in the main firmware and is used
+ by the bootloader to confirm that the firmware is not corrupt and is
+ suitable for this board. The build dependent values in this structure
+ are filled in by set_app_descriptor() in the waf build
+ */
+struct app_descriptor {
+    uint8_t sig[8] = { 0x40, 0xa2, 0xe4, 0xf1, 0x64, 0x68, 0x91, 0x06 };
+    // crc1 is the crc32 from firmware start to start of image_crc1
+    uint32_t image_crc1 = 0;
+    // crc2 is the crc32 from the start of version_major to the end of the firmware
+    uint32_t image_crc2 = 0;
+    // total size of firmware image in bytes
+    uint32_t image_size = 0;
+    uint32_t git_hash = 0;
+    // software version number
+    uint8_t  version_major = APP_FW_MAJOR;
+    uint8_t version_minor = APP_FW_MINOR;
+    // APJ_BOARD_ID (hardware version). This is also used in CAN NodeInfo
+    // with high byte in HardwareVersion.major and low byte in HardwareVersion.minor
+    uint16_t  board_id = APJ_BOARD_ID;
+    uint8_t reserved[8] = { 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff };
+};
+
+#define APP_DESCRIPTOR_TOTAL_LENGTH 36
+static_assert(sizeof(app_descriptor) == APP_DESCRIPTOR_TOTAL_LENGTH, "app_descriptor incorrect length");
+
+#ifdef HAL_BOOTLOADER_BUILD
+check_fw_result_t check_good_firmware(void);
+#endif
+void check_firmware_print(void);
+
+#endif // AP_CHECK_FIRMWARE_ENABLED