Implement reboot to new image

CMakeLists.txt

@@ -16,9 +16,16 @@ set(CMAKE_CXX_FLAGS "${COMMON_FLAGS}")
 set(CMAKE_C_STANDARD 11)
 set(CMAKE_CXX_STANDARD 20)
 
-set(COMMON_LINKER_FLAGS "-specs=nosys.specs -T${CMAKE_SOURCE_DIR}/STM32F103RCTx_FLASH.ld -lc -lm -lnosys -lstdc++ -Wl,--gc-sections -Wl,-Map=${CMAKE_BINARY_DIR}/hover.map,--cref")
-set(CMAKE_EXE_LINKER_FLAGS "${COMMON_LINKER_FLAGS}")
-set(CMAKE_SHARED_LINKER_FLAGS "${COMMON_LINKER_FLAGS}")
+set(A_FIRMWARE_LINKER_FLAGS "-T${CMAKE_SOURCE_DIR}/ld/app_a.ld")
+set(B_FIRMWARE_LINKER_FLAGS "-T${CMAKE_SOURCE_DIR}/ld/app_b.ld")
+set(AB_BOOT_FIRMWARE_LINKER_FLAGS "-T${CMAKE_SOURCE_DIR}/ld/ab_boot.ld")
+set(COMMON_LINKER_FLAGS -specs=nosys.specs -lc -lm -lnosys -lstdc++ -Wl,--gc-sections "-Wl,-Map=${CMAKE_BINARY_DIR}/hover.map,--cref"
+                        -T${CMAKE_SOURCE_DIR}/ld/STM32F103RCTx_FLASH.ld)
+
+set(A_FIRMWARE_APP_BASE 0x08002000)
+set(B_FIRMWARE_APP_BASE 0x08020800)
+
+set(CMAKE_TRY_COMPILE_TARGET_TYPE STATIC_LIBRARY)
 
 project(bobbycar-controller-firmware ASM C CXX)
 
@@ -31,7 +38,6 @@ include_directories(
     STM32CubeF1/Drivers/CMSIS/Device/ST/STM32F1xx/Include
     STM32CubeF1/Drivers/CMSIS/Include
     bobbycar-foc-model
-    bobbycar-protocol
 )
 
 add_library(stm32_hal STATIC
@@ -52,7 +58,6 @@ add_library(stm32_hal STATIC
     STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_dma.c
     STM32CubeF1/Drivers/STM32F1xx_HAL_Driver/Src/stm32f1xx_hal_can.c
     startup_stm32f103xe.s
-    system_stm32f1xx.c
 )
 
 add_library(emanuel_foc_model STATIC
@@ -62,16 +67,23 @@ add_library(emanuel_foc_model STATIC
     bobbycar-foc-model/rtwtypes.h
 )
 
-add_library(bobbycar-protocol INTERFACE
+add_library(bobbycar-protocol INTERFACE)
+target_include_directories(bobbycar-protocol INTERFACE bobbycar-protocol)
+
+
+set(COMMON_SOURCES
+    config.h
+    defines.h
+    main.cpp
+    system_stm32f1xx.c
+    persist/persist.c
 )
 
-
-
-
 #
 # motor test
 #
-add_executable(motortest.elf config.h defines.h main.cpp)
+add_executable(motortest.elf ${COMMON_SOURCES})
+target_link_options(motortest.elf PRIVATE ${COMMON_LINKER_FLAGS} ${A_FIRMWARE_LINKER_FLAGS})
 target_link_libraries(motortest.elf stm32_hal emanuel_foc_model bobbycar-protocol)
 target_compile_options(motortest.elf PRIVATE
     -DMOTOR_TEST
@@ -92,12 +104,13 @@ target_compile_options(motortest.elf PRIVATE
 add_custom_command(OUTPUT motortest.hex COMMAND arm-none-eabi-objcopy -O ihex motortest.elf motortest.hex DEPENDS motortest.elf)
 add_custom_command(OUTPUT motortest.bin COMMAND arm-none-eabi-objcopy -O binary -S motortest.elf motortest.bin DEPENDS motortest.elf)
 add_custom_target(motortest ALL SOURCES motortest.hex motortest.bin)
-add_custom_target(flash-motortest COMMAND st-flash --reset write motortest.bin 0x8000000 SOURCES motortest.bin DEPENDS motortest.bin)
+add_custom_target(flash-motortest COMMAND st-flash --reset write motortest.bin ${A_FIRMWARE_APP_BASE} SOURCES motortest.bin DEPENDS motortest.bin)
 
 #
 # motor test peter
 #
-add_executable(motortest_peter.elf config.h defines.h main.cpp)
+add_executable(motortest_peter.elf ${COMMON_SOURCES})
+target_link_options(motortest_peter.elf PRIVATE ${COMMON_LINKER_FLAGS} ${A_FIRMWARE_LINKER_FLAGS})
 target_link_libraries(motortest_peter.elf stm32_hal emanuel_foc_model bobbycar-protocol)
 target_compile_options(motortest_peter.elf PRIVATE
     #-DMOTOR_TEST
@@ -119,13 +132,14 @@ target_compile_options(motortest_peter.elf PRIVATE
 add_custom_command(OUTPUT motortest_peter.hex COMMAND arm-none-eabi-objcopy -O ihex motortest_peter.elf motortest_peter.hex DEPENDS motortest_peter.elf)
 add_custom_command(OUTPUT motortest_peter.bin COMMAND arm-none-eabi-objcopy -O binary -S motortest_peter.elf motortest_peter.bin DEPENDS motortest_peter.elf)
 add_custom_target(motortest_peter ALL SOURCES motortest_peter.hex motortest_peter.bin)
-add_custom_target(flash-motortest_peter COMMAND st-flash --reset write motortest_peter.bin 0x8000000 SOURCES motortest_peter.bin DEPENDS motortest_peter.bin)
+add_custom_target(flash-motortest_peter COMMAND st-flash --reset write motortest_peter.bin ${A_FIRMWARE_APP_BASE} SOURCES motortest_peter.bin DEPENDS motortest_peter.bin)
 
 
 #
 # feedc0de front
 #
-add_executable(feedcode-front.elf config.h defines.h main.cpp)
+add_executable(feedcode-front.elf ${COMMON_SOURCES})
+target_link_options(feedcode-front.elf PRIVATE ${COMMON_LINKER_FLAGS} ${A_FIRMWARE_LINKER_FLAGS})
 target_link_libraries(feedcode-front.elf stm32_hal emanuel_foc_model bobbycar-protocol)
 target_compile_options(feedcode-front.elf PRIVATE
 #    -DMOTOR_TEST
@@ -146,12 +160,13 @@ target_compile_options(feedcode-front.elf PRIVATE
 add_custom_command(OUTPUT feedcode-front.hex COMMAND arm-none-eabi-objcopy -O ihex feedcode-front.elf feedcode-front.hex DEPENDS feedcode-front.elf)
 add_custom_command(OUTPUT feedcode-front.bin COMMAND arm-none-eabi-objcopy -O binary -S feedcode-front.elf feedcode-front.bin DEPENDS feedcode-front.elf)
 add_custom_target(feedcode-front ALL SOURCES feedcode-front.hex feedcode-front.bin)
-add_custom_target(flash-feedcode-front COMMAND st-flash --reset write feedcode-front.bin 0x8000000 SOURCES feedcode-front.bin DEPENDS feedcode-front.bin)
+add_custom_target(flash-feedcode-front COMMAND st-flash --reset write feedcode-front.bin ${A_FIRMWARE_APP_BASE} SOURCES feedcode-front.bin DEPENDS feedcode-front.bin)
 
 #
 # feedc0de back
 #
-add_executable(feedcode-back.elf config.h defines.h main.cpp)
+add_executable(feedcode-back.elf ${COMMON_SOURCES})
+target_link_options(feedcode-back.elf PRIVATE ${COMMON_LINKER_FLAGS} ${A_FIRMWARE_LINKER_FLAGS})
 target_link_libraries(feedcode-back.elf stm32_hal emanuel_foc_model bobbycar-protocol)
 target_compile_options(feedcode-back.elf PRIVATE
 #    -DMOTOR_TEST
@@ -172,12 +187,13 @@ target_compile_options(feedcode-back.elf PRIVATE
 add_custom_command(OUTPUT feedcode-back.hex COMMAND arm-none-eabi-objcopy -O ihex feedcode-back.elf feedcode-back.hex DEPENDS feedcode-back.elf)
 add_custom_command(OUTPUT feedcode-back.bin COMMAND arm-none-eabi-objcopy -O binary -S feedcode-back.elf feedcode-back.bin DEPENDS feedcode-back.elf)
 add_custom_target(feedcode-back ALL SOURCES feedcode-back.hex feedcode-back.bin)
-add_custom_target(flash-feedcode-back COMMAND st-flash --reset write feedcode-back.bin 0x8000000 SOURCES feedcode-back.bin DEPENDS feedcode-back.bin)
+add_custom_target(flash-feedcode-back COMMAND st-flash --reset write feedcode-back.bin ${A_FIRMWARE_APP_BASE} SOURCES feedcode-back.bin DEPENDS feedcode-back.bin)
 
 #
 # greyhash
 #
-add_executable(greyhash.elf config.h defines.h main.cpp)
+add_executable(greyhash.elf ${COMMON_SOURCES})
+target_link_options(greyhash.elf PRIVATE ${COMMON_LINKER_FLAGS} ${A_FIRMWARE_LINKER_FLAGS})
 target_link_libraries(greyhash.elf stm32_hal emanuel_foc_model bobbycar-protocol)
 target_compile_options(greyhash.elf PRIVATE
 #    -DMOTOR_TEST
@@ -198,10 +214,21 @@ target_compile_options(greyhash.elf PRIVATE
 add_custom_command(OUTPUT greyhash.hex COMMAND arm-none-eabi-objcopy -O ihex greyhash.elf greyhash.hex DEPENDS greyhash.elf)
 add_custom_command(OUTPUT greyhash.bin COMMAND arm-none-eabi-objcopy -O binary -S greyhash.elf greyhash.bin DEPENDS greyhash.elf)
 add_custom_target(greyhash ALL SOURCES greyhash.hex greyhash.bin)
-add_custom_target(flash-greyhash COMMAND st-flash --reset write greyhash.bin 0x8000000 SOURCES greyhash.bin DEPENDS greyhash.bin)
-
+add_custom_target(flash-greyhash COMMAND st-flash --reset write greyhash.bin ${A_FIRMWARE_APP_BASE} SOURCES greyhash.bin DEPENDS greyhash.bin)
 
 
+add_executable(ab_boot.elf
+    ab_boot/ab_boot.c
+    system_stm32f1xx.c
+    persist/persist.c
+)
+target_link_options(ab_boot.elf PRIVATE ${COMMON_LINKER_FLAGS} ${AB_BOOT_FIRMWARE_LINKER_FLAGS})
+target_link_libraries(ab_boot.elf stm32_hal)
+target_compile_options(ab_boot.elf PRIVATE )
+add_custom_command(OUTPUT ab_boot.hex COMMAND arm-none-eabi-objcopy -O ihex ab_boot.elf ab_boot.hex DEPENDS ab_boot.elf)
+add_custom_command(OUTPUT ab_boot.bin COMMAND arm-none-eabi-objcopy -O binary -S ab_boot.elf ab_boot.bin DEPENDS ab_boot.elf)
+add_custom_target(ab_boot ALL SOURCES ab_boot.hex ab_boot.bin)
+add_custom_target(flash-boot COMMAND st-flash --reset write ab_boot.bin 0x08000000 SOURCES ab_boot.bin DEPENDS ab_boot.bin)
 
 
 # util targets

ab_boot/ab_boot.c

@@ -0,0 +1,51 @@
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "stm32f1xx_hal.h"
+#include "ab_boot.h"
+#include "persist/persist.h"
+
+static inline void __attribute__((noreturn)) boot_image(uint32_t sp, uint32_t entry) {
+    asm volatile(
+        "   mov sp, %0    \n"
+        "   blx  %1       \n"
+        "1: b   1b        \n"
+        :
+        : "r" (sp), "r" (entry)
+    );
+    __builtin_unreachable();
+}
+
+static bool is_valid_boot_address(uint32_t *bootp)
+{
+    return (uint32_t)bootp >= FLASH_START &&
+        (uint32_t)bootp < FLASH_END &&
+        ((uint32_t)bootp & 3) == 0;
+}
+
+int main()
+{
+    struct ab_boot_config *flash_config = (struct ab_boot_config *)CONFIG_START;
+    struct ab_boot_config *config = NULL;
+    if (is_persist_valid() &&
+        is_valid_boot_address(persist.ab_boot_config.boot_partition))
+    {
+        // Invalidate persist
+        persist.checksum = 0;
+        config = &persist.ab_boot_config;
+    }
+    else if (is_valid_boot_address(flash_config->boot_partition))
+    {
+        config = flash_config;
+    }
+
+    uint32_t *bootp;
+    if (config)
+        bootp = config->boot_partition;
+    else
+        bootp = (uint32_t *)APP_A_START;
+
+    uint32_t sp = bootp[0];
+    uint32_t entry = bootp[1];
+    boot_image(sp, entry);
+}

ab_boot/ab_boot.h

@@ -0,0 +1,21 @@
+#pragma once
+
+#include <stdint.h>
+
+#define FLASH_START  0x08000000
+#define AB_BOOT_SIZE 0x00002000
+
+#define APP_A_START  0x08002000
+#define APP_B_START  0x08020800
+                     // 122 KiB
+#define APP_SIZE        0x1e800
+
+#define CONFIG_START 0x0803f800
+#define CONFIG_SIZE       0x800
+
+#define FLASH_END    0x08040000
+
+struct ab_boot_config
+{
+    uint32_t *boot_partition;
+};

config_partition.h

@@ -0,0 +1,5 @@
+#include "ab_boot/ab_boot.h"
+
+struct config_partition {
+    struct ab_boot_config ab_boot_config;
+};

flasher/CANFlasher.hpp

@@ -0,0 +1,96 @@
+#include <cstddef>
+#include <cstdint>
+
+#include "Flasher.hpp"
+
+namespace can_flasher {
+    namespace {
+        flasher::Result handle_start(uint8_t *data, size_t length) {
+            uintptr_t address;
+            if (length != sizeof(address))
+                return flasher::Result::InvalidParameter;
+
+            memcpy(&address, data, sizeof(address));
+            return flasher::start(address);
+        }
+
+        flasher::Result handle_write(uint8_t *data, size_t length) {
+            return flasher::write(data, length);
+        }
+    }
+
+    void handle(uint8_t *data, size_t length) {
+        using enum flasher::State;
+
+        if (length < 1) {
+            flasher::flasher_state_callback(flasher::get_state(), flasher::Result::InvalidParameter, 0);
+            return;
+        }
+
+        flasher::State target_state = (flasher::State)*data;
+        data += 1;
+        length -= 1;
+
+        flasher::Result result;
+        switch (target_state) {
+        case Idle:
+            flasher::init();
+            result = flasher::Result::Success;
+            break;
+        case Erasing:
+            result = handle_start(data, length);
+            break;
+        case Writing:
+            result = handle_write(data, length);
+            break;
+        default:
+            result = flasher::Result::InvalidParameter;
+        }
+
+        flasher::flasher_state_callback(flasher::get_state(), result, 0);
+    }
+
+    constexpr size_t FeedbackSize =
+        sizeof(flasher::State) + sizeof(flasher::Result) + sizeof(uint32_t);
+    static_assert(FeedbackSize <= 8);
+
+    struct {
+        uint32_t last_sent;
+        std::atomic<bool> updated;
+        bool valid;
+        uint8_t data[FeedbackSize];
+    } feedback;
+
+    void generate_feedback(flasher::State state, flasher::Result result, uint32_t arg) {
+        uint8_t *ptr = feedback.data;
+        std::memcpy(ptr, &state, sizeof(state));
+        ptr += sizeof(state);
+        std::memcpy(ptr, &result, sizeof(result));
+        ptr += sizeof(result);
+        std::memcpy(ptr, &arg, sizeof(arg));
+        // this works because poll_feedback can't interrupt us
+        feedback.updated.store(true);
+    }
+
+    bool poll_feedback(uint32_t now, uint8_t *out) {
+        if (feedback.updated.load() ||
+            (feedback.valid && now - feedback.last_sent >= 500)) {
+            feedback.valid = true;
+            do {
+                feedback.updated.store(false);
+                std::memcpy(out, feedback.data, sizeof(feedback.data));
+                // this works because we cannot interrupt generate_feedback
+            } while (feedback.updated.load());
+            feedback.last_sent = now;
+            return true;
+        }
+
+        return false;
+    }
+}
+
+namespace flasher {
+void flasher_state_callback(flasher::State state, flasher::Result result, uint32_t arg) {
+    can_flasher::generate_feedback(state, result, arg);
+}
+}

flasher/Flasher.hpp

@@ -0,0 +1,203 @@
+#pragma once
+
+#include <cstdint>
+#include <cstring>
+
+#include "stm32f1xx_hal.h"
+
+#include "ab_boot/ab_boot.h"
+#include "stm32f1xx_hal_def.h"
+#include "stm32f1xx_hal_flash.h"
+#include "stm32f1xx_hal_flash_ex.h"
+
+
+namespace flasher {
+    enum class State : uint8_t {
+        Idle,
+        Erasing,
+        Waiting,
+        Writing,
+        Error
+    };
+
+    namespace {
+        enum class FlashRegion {
+            Invalid,
+            Bootloader,
+            AppA,
+            AppB,
+            Config
+        };
+
+        FlashRegion region_for_address(uintptr_t address) {
+            using enum FlashRegion;
+
+            if (address >= FLASH_START && address < APP_A_START) {
+                return Bootloader;
+            } else if (address >= APP_A_START && address < (APP_A_START + APP_SIZE)) {
+                return AppA;
+            } else if (address >= APP_B_START && address < (APP_B_START + APP_SIZE)) {
+                return AppB;
+            } else if (address >= CONFIG_START && address < (CONFIG_START + CONFIG_SIZE)) {
+                return Config;
+            }
+
+            return Invalid;
+        }
+
+        size_t region_size(FlashRegion region) {
+            using enum FlashRegion;
+
+            switch (region) {
+            case Bootloader:
+                return AB_BOOT_SIZE;
+            case AppA:
+            case AppB:
+                return APP_SIZE;
+            case Config:
+                return CONFIG_SIZE;
+            default:
+                return 0;
+            }
+        }
+
+        bool is_valid_start_address(uint32_t address) {
+            return address == FLASH_START || address == APP_A_START ||
+                   address == APP_B_START || address == CONFIG_START;
+        }
+
+        static State state_;
+        static FlashRegion region_;
+        static uintptr_t address_;
+        static uint8_t write_size_;
+    }
+
+    enum class Result : uint8_t {
+        Success,
+        RegionNotAllowed,
+        InvalidParameter,
+        InvalidState,
+        WriteError,
+        InProgress
+    };
+
+    void flasher_state_callback(State state, Result result, uint32_t arg);
+
+    void init() {
+        state_ = State::Idle;
+        region_ = FlashRegion::Invalid;
+        address_ = 0;
+        write_size_ = 0;
+        HAL_FLASH_Lock();
+    }
+
+    Result start(uintptr_t address) {
+        if (state_ != State::Idle)
+            return Result::InvalidState;
+
+        if (!is_valid_start_address(address)) {
+            return Result::InvalidParameter;
+        }
+
+        FlashRegion flashed_region = region_for_address(address);
+        FlashRegion running_region = region_for_address((uintptr_t)&start);
+
+        if (flashed_region == FlashRegion::Invalid) {
+            return Result::InvalidParameter;
+        }
+
+        if (flashed_region == running_region) {
+            // prohibit flashing the currently running app
+            return Result::RegionNotAllowed;
+        }
+
+        size_t size = region_size(flashed_region);
+        FLASH_EraseInitTypeDef ferase = {
+            .TypeErase = FLASH_TYPEERASE_PAGES,
+            .PageAddress = address,
+            .NbPages = size / FLASH_PAGE_SIZE
+        };
+
+        HAL_FLASH_Unlock();
+        if (HAL_FLASHEx_Erase_IT(&ferase) != HAL_OK) {
+            return Result::WriteError;
+        }
+
+        state_ = State::Erasing;
+        region_ = flashed_region;
+        address_ = address;
+
+        return Result::InProgress;
+    }
+
+    Result write(uint8_t *data, uint8_t length) {
+        if (state_ != State::Waiting)
+            return Result::InvalidState;
+
+        uint32_t program_type;
+        switch (length) {
+        case 2:
+            program_type = FLASH_PROC_PROGRAMHALFWORD;
+            break;
+        case 4:
+            program_type = FLASH_PROC_PROGRAMWORD;
+            break;
+        case 8:
+            program_type = FLASH_PROC_PROGRAMDOUBLEWORD;
+            break;
+        default:
+            return Result::InvalidParameter;
+        }
+
+        uint64_t data_int = 0;
+        memcpy(&data_int, data, length);
+
+        if (HAL_FLASH_Program_IT(program_type, address_, data_int) != HAL_OK)
+            return Result::WriteError;
+
+        state_ = State::Writing;
+        write_size_ = length;
+
+        return Result::InProgress;
+    }
+
+    State get_state() {
+        return state_;
+    }
+
+    void flash_callback(bool success) {
+        using enum State;
+
+        // Ignore if we are in Idle state, could be the result of
+        // a cancelled operation.
+        if (state_ == Idle)
+            return;
+
+        if (success) {
+            switch (state_) {
+            case Writing:
+            case Erasing:
+                address_ += write_size_;
+                flasher_state_callback(state_, Result::Success, address_);
+                state_ = Waiting;
+                write_size_ = 0;
+                break;
+            default:
+                // Spurious callback
+                HAL_FLASH_Lock();
+                state_ = Error;
+            }
+        } else {
+            switch (state_) {
+            case Writing:
+            case Erasing:
+                flasher_state_callback(state_, Result::WriteError, address_);
+            [[fallthrough]];
+            default:
+                // Spurious callback
+                HAL_FLASH_Lock();
+                state_ = Error;
+            }
+        }
+    }
+}

ld/STM32F103RCTx_FLASH.ld

@@ -32,8 +32,6 @@
 /* Entry Point */
 ENTRY(Reset_Handler)
 
-/* Highest address of the user mode stack */
-_estack = 0x2000C000;    /* end of RAM */
 /* Generate a link error if heap and stack don't fit into RAM */
 _Min_Heap_Size = 0x200;      /* required amount of heap  */
 _Min_Stack_Size = 0x400; /* required amount of stack */
@@ -41,8 +39,10 @@ _Min_Stack_Size = 0x400; /* required amount of stack */
 /* Specify the memory areas */
 MEMORY
 {
-RAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 48K
-FLASH (rx)      : ORIGIN = 0x8000000, LENGTH = 256K
+RAM (xrw)      : ORIGIN = 0x20000000, LENGTH = 47K
+PERSIST (rw)   : ORIGIN = 0x2000BC00, LENGTH =  1K
+FLASH (rx)     : ORIGIN = _App_Base,  LENGTH =  _App_Length
+CONFIG(r)      : ORIGIN = 0x0803f800, LENGTH =  2K
 }
 
 /* Define output sections */
@@ -114,7 +114,7 @@ SECTIONS
   _sidata = LOADADDR(.data);
 
   /* Initialized data sections goes into RAM, load LMA copy after code */
-  .data : 
+  .data :
   {
     . = ALIGN(4);
     _sdata = .;        /* create a global symbol at data start */
@@ -125,7 +125,7 @@ SECTIONS
     _edata = .;        /* define a global symbol at data end */
   } >RAM AT> FLASH
 
-  
+
   /* Uninitialized data section */
   . = ALIGN(4);
   .bss :
@@ -153,7 +153,16 @@ SECTIONS
     . = ALIGN(8);
   } >RAM
 
-  
+  /* Highest address of the user mode stack */
+  _estack = ORIGIN(RAM) + LENGTH(RAM);    /* end of RAM area */
+
+  .persist :
+  {
+    . = ALIGN(4);
+    persist = .;
+    . += LENGTH(PERSIST);
+    _epersist = .;
+  } > PERSIST
 
   /* Remove information from the standard libraries */
   /DISCARD/ :
@@ -165,5 +174,3 @@ SECTIONS
 
   .ARM.attributes 0 : { *(.ARM.attributes) }
 }
-
-

ld/ab_boot.ld

@@ -0,0 +1,2 @@
+_App_Base = 0x08000000;
+_App_Length = 8K;

ld/app_a.ld

@@ -0,0 +1,2 @@
+_App_Base = 0x08002000;
+_App_Length = 122K;

ld/app_b.ld

@@ -0,0 +1,2 @@
+_App_Base = 0x08020800;
+_App_Length = 122K;

main.cpp

@@ -27,6 +27,8 @@
 #include <cstring>
 
 #include "stm32f1xx_hal.h"
+#include "ab_boot/ab_boot.h"
+#include "persist/persist.h"
 
 #include "defines.h"
 #include "config.h"
@@ -37,6 +39,8 @@
 #endif
 #ifdef FEATURE_CAN
 #include "bobbycar-can.h"
+#include "flasher/Flasher.hpp"
+#include "flasher/CANFlasher.hpp"
 #endif
 
 extern "C" {
@@ -110,7 +114,7 @@ CAN_HandleTypeDef     CanHandle;
 #define CANx_TX_IRQn                   USB_HP_CAN1_TX_IRQn
 #define CANx_TX_IRQHandler             USB_HP_CAN1_TX_IRQHandler
 
-constexpr bool doDelayWithCanPoll = false;
+constexpr bool doDelayWithCanPoll = true;
 #endif
 
 #ifdef LOG_TO_SERIAL
@@ -174,6 +178,8 @@ protocol::serial::Feedback feedback;
 #ifdef FEATURE_CAN
 std::atomic<int16_t> timeoutCntLeft   = 0;
 std::atomic<int16_t> timeoutCntRight  = 0;
+
+uint32_t *reboot_request_address = 0;
 #endif
 
 uint32_t main_loop_counter;
@@ -247,6 +253,8 @@ void sendFeedback();
 void parseCanCommand();
 void applyIncomingCanMessage();
 void sendCanFeedback();
+void sendFlasherFeedback();
+void handleFlasher();
 #endif
 
 #ifdef FEATURE_BUTTON
@@ -377,6 +385,7 @@ int main()
                 while ((HAL_GetTick() - tickstart) < wait)
                 {
                     applyIncomingCanMessage();
+                    sendFlasherFeedback();
                 }
             };
 
@@ -404,6 +413,7 @@ int main()
         parseCanCommand();
 
         sendCanFeedback();
+        handleFlasher();
 #endif
 
 #ifdef FEATURE_BUTTON
@@ -448,6 +458,7 @@ void updateMotors()
     if (offsetcount < 2000) // calibrate ADC offsets
     {
         offsetcount++;
+        // TODO this is not an average
         offsetrl1 = (adc_buffer.rl1 + offsetrl1) / 2;
         offsetrl2 = (adc_buffer.rl2 + offsetrl2) / 2;
         offsetrr1 = (adc_buffer.rr1 + offsetrr1) / 2;
@@ -1364,22 +1375,36 @@ void MX_ADC2_Init()
     __HAL_ADC_ENABLE(&hadc2);
 }
 
-#ifdef FEATURE_BUTTON
-void poweroff()
+void shutdown()
 {
-//  if (abs(speed) < 20) {  // wait for the speed to drop, then shut down -> this is commented out for SAFETY reasons
     buzzer.pattern = 0;
     left.enable = false;
-    right.enable = 0;
+    right.enable = false;
 
     for (int i = 0; i < 8; i++) {
         buzzer.freq = (uint8_t)i;
         HAL_Delay(50);
     }
+}
+
+#ifdef FEATURE_BUTTON
+void poweroff()
+{
+    shutdown();
+
     HAL_GPIO_WritePin(OFF_PORT, OFF_PIN, GPIO_PIN_RESET);
     for (int i = 0; i < 5; i++)
         HAL_Delay(1000);
-//  }
+}
+#endif
+
+#ifdef FEATURE_CAN
+void reboot_new_image(uint32_t *bootp)
+{
+    shutdown();
+
+    request_boot_image(bootp);
+    HAL_NVIC_SystemReset();
 }
 #endif
 
@@ -1388,8 +1413,8 @@ void communicationTimeout()
 {
     applyDefaultSettings();
 
-    buzzer.freq = 24;
-    buzzer.pattern = 1;
+    //buzzer.freq = 24;
+    //buzzer.pattern = 1;
 
     HAL_GPIO_WritePin(LED_PORT, LED_PIN, GPIO_PIN_RESET);
 }
@@ -1648,11 +1673,21 @@ void applyIncomingCanMessage()
         break;
     case MotorController<isBackBoard, false>::Command::Poweroff:
     case MotorController<isBackBoard, true>::Command::Poweroff:
+        {
+            if (header.DLC >= 2)
+            {
+                // Reboot selected image
+                reboot_request_address = (uint32_t *)((*(uint8_t *)buf == 0) ? APP_A_START : APP_B_START);
+            }
+            else
+            {
 #ifdef FEATURE_BUTTON
-        if (*((bool*)buf))
-            poweroff();
+                if (*((bool*)buf))
+                    poweroff();
 #endif
-        break;
+            }
+            break;
+        }
     default:
 #ifndef CAN_LOG_UNKNOWN_ADDR
         if constexpr (false)
@@ -1751,6 +1786,41 @@ void sendCanFeedback()
 
     arr[whichToSend++]();
 }
+
+void sendFlasherFeedback() {
+    using bobbycar::protocol::can::MotorController;
+    static CAN_TxHeaderTypeDef header = {
+        .StdId = MotorController<isBackBoard, false>::Command::FlasherCtrl,
+        .ExtId = 0,
+        .IDE = CAN_ID_STD,
+        .RTR = CAN_RTR_DATA,
+        .DLC = can_flasher::FeedbackSize,
+        .TransmitGlobalTime = DISABLE
+    };
+
+    if (HAL_CAN_GetTxMailboxesFreeLevel(&CanHandle) == 0)
+        return;
+
+    uint8_t buf[8];
+    if (!can_flasher::poll_feedback(HAL_GetTick(), buf))
+        return;
+
+    uint32_t TxMailbox;
+    if (const auto result = HAL_CAN_AddTxMessage(&CanHandle, &header, buf, &TxMailbox); result != HAL_OK) {
+        myPrintf("HAL_CAN_AddTxMessage() failed with %i", result);
+        //while (true);
+    }
+}
+
+void handleFlasher()
+{
+    sendFlasherFeedback();
+
+    if (reboot_request_address)
+    {
+        reboot_new_image(reboot_request_address);
+    }
+}
 #endif
 
 #ifdef FEATURE_BUTTON
@@ -2045,4 +2115,25 @@ extern "C" void CANx_TX_IRQHandler(void)
     using namespace bobbycar::controller;
     HAL_CAN_IRQHandler(&CanHandle);
 }
+
+// CAN flasher stuff
+extern "C" void FLASH_IRQHandler(void)
+{
+    HAL_FLASH_IRQHandler();
+}
+
+extern "C" void HAL_FLASH_EndOfOperationCallback(uint32_t ReturnValue)
+{
+    (void)ReturnValue;
+
+    flasher::flash_callback(true);
+}
+
+extern "C" void HAL_FLASH_OperationErrorCallback(uint32_t ReturnValue)
+{
+    (void)ReturnValue;
+
+    flasher::flash_callback(false);
+}
+
 #endif

persist/persist.c

@@ -0,0 +1,10 @@
+#include <assert.h>
+
+#include "persist.h"
+#include "ab_boot/ab_boot.h"
+
+// keep this in sync with the linker script
+#define PERSIST_SIZE 1024
+
+extern struct persist_data persist;
+static_assert(sizeof(persist) < PERSIST_SIZE);

persist/persist.h

@@ -0,0 +1,39 @@
+#pragma once
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include "ab_boot/ab_boot.h"
+
+struct persist_data {
+    struct ab_boot_config ab_boot_config;
+    uint32_t checksum;
+};
+
+extern struct persist_data persist;
+
+static inline uint32_t calculate_persist_checksum() {
+    uint32_t checksum = 0;
+    uint32_t *pd = (uint32_t *)&persist;
+    for (int i = 0; i < (sizeof(persist) - 4) / 4; i++) {
+        checksum += pd[i];
+        checksum = (checksum >> 3) | (checksum << 29);
+    }
+
+    checksum = ~checksum;
+
+    return checksum;
+}
+
+static inline void update_persist_checksum() {
+    persist.checksum = calculate_persist_checksum();
+}
+
+static inline bool is_persist_valid() {
+    return calculate_persist_checksum() == persist.checksum;
+}
+
+static inline void request_boot_image(uint32_t *bootp) {
+    persist.ab_boot_config.boot_partition = bootp;
+    update_persist_checksum();
+}

system_stm32f1xx.c

@@ -110,10 +110,10 @@
 /*!< Uncomment the following line if you need to relocate your vector Table in
      Internal SRAM. */
 /* #define VECT_TAB_SRAM */
-#define VECT_TAB_OFFSET 0x00000000U /*!< Vector Table base offset field. \
+extern unsigned int g_pfnVectors[];
+#define VECT_TAB_OFFSET ((unsigned int)g_pfnVectors) /*!< Vector Table base offset field. \
                                  This value must be a multiple of 0x200. */
 
-
 /**
   * @}
   */