From d9045997a07f11c90ae05a28fde35e872b1a6fd8 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 18 Aug 2022 16:45:30 +1000 Subject: [PATCH] AP_Bootloader: add SD card support to bootloader --- Tools/AP_Bootloader/AP_Bootloader.cpp | 7 + Tools/AP_Bootloader/AP_Bootloader_config.h | 7 + Tools/AP_Bootloader/flash_from_sd.cpp | 385 +++++++++++++++++++++ Tools/AP_Bootloader/flash_from_sd.h | 12 + Tools/AP_Bootloader/md5.cpp | 6 +- Tools/AP_Bootloader/md5.h | 6 + 6 files changed, 422 insertions(+), 1 deletion(-) create mode 100644 Tools/AP_Bootloader/AP_Bootloader_config.h create mode 100644 Tools/AP_Bootloader/flash_from_sd.cpp create mode 100644 Tools/AP_Bootloader/flash_from_sd.h diff --git a/Tools/AP_Bootloader/AP_Bootloader.cpp b/Tools/AP_Bootloader/AP_Bootloader.cpp index abe4c0c9cb..26743bd9fa 100644 --- a/Tools/AP_Bootloader/AP_Bootloader.cpp +++ b/Tools/AP_Bootloader/AP_Bootloader.cpp @@ -30,6 +30,7 @@ #include #include "support.h" #include "bl_protocol.h" +#include "flash_from_sd.h" #include "can.h" #include #if EXT_FLASH_SIZE_MB @@ -181,6 +182,12 @@ int main(void) } #endif +#if AP_BOOTLOADER_FLASH_FROM_SD_ENABLED + if (flash_from_sd()) { + jump_to_app(); + } +#endif + #if defined(BOOTLOADER_DEV_LIST) while (true) { bootloader(timeout); diff --git a/Tools/AP_Bootloader/AP_Bootloader_config.h b/Tools/AP_Bootloader/AP_Bootloader_config.h new file mode 100644 index 0000000000..459fca73cd --- /dev/null +++ b/Tools/AP_Bootloader/AP_Bootloader_config.h @@ -0,0 +1,7 @@ +#pragma once + +#include "hwdef.h" + +#ifndef AP_BOOTLOADER_FLASH_FROM_SD_ENABLED +#define AP_BOOTLOADER_FLASH_FROM_SD_ENABLED 0 +#endif diff --git a/Tools/AP_Bootloader/flash_from_sd.cpp b/Tools/AP_Bootloader/flash_from_sd.cpp new file mode 100644 index 0000000000..c053e48252 --- /dev/null +++ b/Tools/AP_Bootloader/flash_from_sd.cpp @@ -0,0 +1,385 @@ +#include "flash_from_sd.h" + +#if AP_BOOTLOADER_FLASH_FROM_SD_ENABLED + +#include "ch.h" +#include "ff.h" + +#include "md5.h" + +#include +#include +#include +#include +#include "stm32_util.h" + +#include +#include +#include "support.h" + +// swiped from support.cpp: +static const uint8_t *flash_base = (const uint8_t *)(0x08000000 + (FLASH_BOOTLOADER_LOAD_KB + APP_START_OFFSET_KB)*1024U); + + +// taken from AP_Common.cpp as we don't want to compile the AP_Common +// directory. This function is defined in AP_Common.h - so we can't +// use "static" here. +/** + * return the numeric value of an ascii hex character + * + * @param[in] a Hexadecimal character + * @return Returns a binary value + */ +int16_t char_to_hex(char a) +{ + if (a >= 'A' && a <= 'F') + return a - 'A' + 10; + else if (a >= 'a' && a <= 'f') + return a - 'a' + 10; + else + return a - '0'; +} + +#define MAX_IO_SIZE 4096 +static uint8_t buffer[MAX_IO_SIZE]; + +// a class which provides parsing functionality for abin files; +// inheritors must supply a function to deal with the body of the abin +// and may supply methods run() (to initialise their state) and +// name_value_callback (to handle name/value pairs extracted from the +// abin header. +class ABinParser { +public: + ABinParser(const char *_path) : + path{_path} + { } + virtual ~ABinParser() {} + + virtual bool run() = 0; + +protected: + + virtual void name_value_callback(const char *name, const char *value) {} + virtual void body_callback(const uint8_t *bytes, uint32_t n) = 0; + bool parse(); + +private: + + const char *path; + +}; + +bool ABinParser::parse() +{ + FIL fh; + if (f_open(&fh, path, FA_READ) != FR_OK) { + return false; + } + enum class State { + START_NAME=17, // "MD5: " + ACCUMULATING_NAME=19, + ACCUMULATING_VALUE = 30, + START_BODY = 40, + PROCESSING_BODY = 45, + SKIPPING_POST_COLON_SPACES = 50, + START_VALUE = 55, + }; + + State state = State::START_NAME; + uint16_t name_start = 0; + uint16_t name_end = 0; + uint16_t value_start = 0; + // for efficiency we assume all headers are within the first chunk + // read i.e. the name/value pairs do not cross a MAX_IO_SIZE + // boundary + while (true) { + UINT bytes_read; + if (f_read(&fh, buffer, sizeof(buffer), &bytes_read) != FR_OK) { + return false; + } + if (bytes_read > sizeof(buffer)) { + // error + return false; + } + if (bytes_read == 0) { + break; + } + for (uint16_t i=0; i= 3) { + if (!strncmp((char*)&buffer[i], "--\n", bytes_read-i)) { + // end of headers + i += 2; + state = State::START_BODY; + continue; + } + } + // sanity check input: + if (buffer[i] == ':') { + // zero-length name?! just say no: + return false; + } + if (buffer[i] == '\n') { + // empty line... ignore it + continue; + } + name_start = i; + state = State::ACCUMULATING_NAME; + continue; + } + case State::ACCUMULATING_NAME: { + if (buffer[i] == '\n') { + // no colon on this line; just say no: + return false; + } + if (buffer[i] == ':') { + name_end = i; + state = State::SKIPPING_POST_COLON_SPACES; + continue; + } + // continue to accumulate name + continue; + } + case State::SKIPPING_POST_COLON_SPACES: + if (buffer[i] == ' ') { + // continue to accumulate spaces + continue; + } + state = State::START_VALUE; + FALLTHROUGH; + case State::START_VALUE: + value_start = i; + state = State::ACCUMULATING_VALUE; + FALLTHROUGH; + case State::ACCUMULATING_VALUE: { + if (buffer[i] != '\n') { + // continue to accumate value bytes + continue; + } + char name[80]; + char value[80]; + strncpy(name, (char*)&buffer[name_start], MIN(sizeof(name)-1, name_end-name_start)); + strncpy(value, (char*)&buffer[value_start], MIN(sizeof(value)-1, i-value_start)); + name_value_callback(name, value); + state = State::START_NAME; + continue; + } + case State::START_BODY: + state = State::PROCESSING_BODY; + FALLTHROUGH; + case State::PROCESSING_BODY: + body_callback(&buffer[i], bytes_read-i); + i = bytes_read; + continue; + } + } + } + + // successfully parsed the abin. Call the body callback once more + // with zero bytes indicating EOF: + body_callback((uint8_t*)"", 0); + + return true; +} + +// a sub-class of ABinParser which takes the supplied MD5 from the +// abin header and compares it against the calculated md5sum of the +// abin body +class ABinVerifier : ABinParser { +public: + + using ABinParser::ABinParser; + + bool run() override { + MD5Init(&md5_context); + + if (!parse()) { + return false; + } + + // verify the checksum is as expected + uint8_t calculated_md5[16]; + MD5Final(calculated_md5, &md5_context); + if (!memcmp(calculated_md5, expected_md5, sizeof(calculated_md5))) { + // checksums match + return true; + } + + return false; + } + +protected: + + void name_value_callback(const char *name, const char *value) override { + if (strncmp(name, "MD5", 3)) { + // only interested in MD5 header + return; + } + + // convert from 32-byte-string to 16-byte number: + for (uint8_t j=0; j<16; j++) { + expected_md5[j] = (char_to_hex(value[j*2]) << 4) | char_to_hex(value[j*2+1]); + } + } + + void body_callback(const uint8_t *bytes, uint32_t nbytes) override { + MD5Update(&md5_context, bytes, nbytes); + } + +private: + + uint8_t expected_md5[16]; + MD5Context md5_context; +}; + + +// a sub-class of ABinParser which flashes the body of the supplied abin +class ABinFlasher : public ABinParser { +public: + using ABinParser::ABinParser; + + bool run() override { + // start by erasing all sectors + for (uint8_t i = 0; flash_func_sector_size(i) != 0; i++) { + if (!flash_func_erase_sector(i)) { + return false; + } + led_toggle(LED_BOOTLOADER); + } + + // parse and flash + if (!parse()) { + return false; + } + + return !failed; + } + +protected: + + void body_callback(const uint8_t *bytes, uint32_t nbytes) override { + if (failed) { + return; + } + + memcpy(&buffer[buffer_ofs], bytes, nbytes); + buffer_ofs += nbytes; + + const uint32_t WRITE_CHUNK_SIZE = 32*1024; // must be less than size of state buffer + // nbytes is zero after the last chunk in the body + if (buffer_ofs > WRITE_CHUNK_SIZE || nbytes == 0) { + uint32_t write_size = WRITE_CHUNK_SIZE; + uint32_t padded_write_size = write_size; + if (nbytes == 0) { + // final chunk. We have to align to 128 bytes + write_size = buffer_ofs; + padded_write_size = write_size; + const uint8_t pad_size = 128 - (write_size % 128); + // zero those extra bytes: + memset(&buffer[buffer_ofs], '\0', pad_size); + padded_write_size += pad_size; + } + const uint32_t ofs = uint32_t(flash_base) + flash_ofs; + if (!stm32_flash_write(ofs, buffer, padded_write_size)) { + failed = true; + return; + } + flash_ofs += padded_write_size; + buffer_ofs -= write_size; + memcpy(buffer, &buffer[write_size], buffer_ofs); + led_toggle(LED_BOOTLOADER); + } + } + +private: + + uint32_t flash_ofs = 0; + uint32_t buffer_ofs = 0; + uint8_t buffer[64*1024]; // constrained by memory map on bootloader + bool failed = false; +}; + + +// main entry point to the flash-from-sd-card functionality; called +// from the bootloader main function +bool flash_from_sd() +{ + peripheral_power_enable(); + + if (!sdcard_init()) { + return false; + } + + bool ret = false; + + // expected filepath for abin: + const char *abin_path = "/ardupilot.abin"; + // we rename to this before verifying the abin: + const char *verify_abin_path = "/ardupilot-verify.abin"; + // we rename to this before flashing the abin: + const char *flash_abin_path = "/ardupilot-flash.abin"; + // we rename to this after flashing the abin: + const char *flashed_abin_path = "/ardupilot-flashed.abin"; + + ABinVerifier *verifier = nullptr; + ABinFlasher *flasher = nullptr; + + FILINFO info; + if (f_stat(abin_path, &info) != FR_OK) { + goto out; + } + + f_unlink(verify_abin_path); + f_unlink(flash_abin_path); + f_unlink(flashed_abin_path); + + // rename the file so we only ever attempt to flash from it once: + if (f_rename(abin_path, verify_abin_path) != FR_OK) { + // we would be nice to indicate an error here. + // we could try to drop a message on the SD card? + return false; + } + + verifier = new ABinVerifier{verify_abin_path}; + if (!verifier->run()) { + goto out; + } + + // rename the file so we only ever attempt to flash from it once: + if (f_rename(verify_abin_path, flash_abin_path) != FR_OK) { + // we would be nice to indicate an error here. + // we could try to drop a message on the SD card? + return false; + } + + flasher = new ABinFlasher{flash_abin_path}; + if (!flasher->run()) { + goto out; + } + + // rename the file to indicate successful flash: + if (f_rename(flash_abin_path, flashed_abin_path) != FR_OK) { + // we would be nice to indicate an error here. + // we could try to drop a message on the SD card? + return false; + } + + ret = true; + +out: + + delete verifier; + verifier = nullptr; + + delete flasher; + flasher = nullptr; + + sdcard_stop(); + // should we disable peripheral power again?! + + return ret; +} + +#endif // AP_BOOTLOADER_FLASH_FROM_SD_ENABLED diff --git a/Tools/AP_Bootloader/flash_from_sd.h b/Tools/AP_Bootloader/flash_from_sd.h new file mode 100644 index 0000000000..112fd054df --- /dev/null +++ b/Tools/AP_Bootloader/flash_from_sd.h @@ -0,0 +1,12 @@ +#pragma once + +#include "AP_Bootloader_config.h" + +#if AP_BOOTLOADER_FLASH_FROM_SD_ENABLED + +#include +#include + +bool flash_from_sd(); + +#endif // AP_BOOTLOADER_FLASH_FROM_SD_ENABLED diff --git a/Tools/AP_Bootloader/md5.cpp b/Tools/AP_Bootloader/md5.cpp index ef0b80beee..43c01fa565 100644 --- a/Tools/AP_Bootloader/md5.cpp +++ b/Tools/AP_Bootloader/md5.cpp @@ -18,7 +18,11 @@ /* This code slightly modified to fit into Samba by abartlet@samba.org Jun 2001 */ -#include "includes.h" +// swiped and modified from tridge's junk code repository -pb20220818 +#include "md5.h" +#include + +#pragma GCC diagnostic ignored "-Wcast-align" static void MD5Transform(uint32 buf[4], uint32 const in[16]); diff --git a/Tools/AP_Bootloader/md5.h b/Tools/AP_Bootloader/md5.h index 6665171e7c..625de6a27b 100644 --- a/Tools/AP_Bootloader/md5.h +++ b/Tools/AP_Bootloader/md5.h @@ -5,6 +5,12 @@ #define HEADER_MD5_H #endif +// swiped and modified from tridge's junk code repository -pb20220818 +#include "stdint.h" +#define uint32 uint32_t +// ignore cast errors in this case to keep complexity down +// on x86 where replay is run we don't care about cast alignment + struct MD5Context { uint32 buf[4]; uint32 bits[2];