diff --git a/libraries/AP_CheckFirmware/AP_CheckFirmware.cpp b/libraries/AP_CheckFirmware/AP_CheckFirmware.cpp new file mode 100644 index 0000000000..a04cf6a58b --- /dev/null +++ b/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 +#include + +#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 diff --git a/libraries/AP_CheckFirmware/AP_CheckFirmware.h b/libraries/AP_CheckFirmware/AP_CheckFirmware.h new file mode 100644 index 0000000000..13cf4c65dc --- /dev/null +++ b/libraries/AP_CheckFirmware/AP_CheckFirmware.h @@ -0,0 +1,71 @@ +/* + support checking board ID and firmware CRC in the bootloader + */ +#pragma once + +#include +#include + +#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