AP_Bootloader: add SD card support to bootloader

Tools/AP_Bootloader/AP_Bootloader.cpp

@@ -30,6 +30,7 @@
 #include <AP_HAL_ChibiOS/hwdef/common/watchdog.h>
 #include "support.h"
 #include "bl_protocol.h"
+#include "flash_from_sd.h"
 #include "can.h"
 #include <stdio.h>
 #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);

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

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 <string.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <fcntl.h>
+#include "stm32_util.h"
+
+#include <AP_HAL_ChibiOS/hwdef/common/flash.h>
+#include <AP_Math/AP_Math.h>
+#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<bytes_read; i++) {
+            switch (state) {
+            case State::START_NAME: {
+                // check for delimiter:
+                if (bytes_read-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

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 <AP_HAL_ChibiOS/sdcard.h>
+#include <stdbool.h>
+
+bool flash_from_sd();
+
+#endif  // AP_BOOTLOADER_FLASH_FROM_SD_ENABLED

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 <string.h>
+
+#pragma GCC diagnostic ignored "-Wcast-align"
 
 static void MD5Transform(uint32 buf[4], uint32 const in[16]);
 

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];