diff --git a/platforms/chibios/bootloaders/halfkay.c b/platforms/chibios/bootloaders/halfkay.c index aa11e6c5f43..83fc5fd81e0 100644 --- a/platforms/chibios/bootloaders/halfkay.c +++ b/platforms/chibios/bootloaders/halfkay.c @@ -19,9 +19,27 @@ #include #include "wait.h" +#define SCB_AIRCR_VECTKEY_WRITEMAGIC 0x05FA0000 + __attribute__((weak)) void bootloader_jump(void) { wait_ms(100); __BKPT(0); } -__attribute__((weak)) void mcu_reset(void) {} +__attribute__((weak)) void mcu_reset(void) { + /* Ensure all outstanding memory operations complete */ + __DSB(); + __ISB(); + + /* Request system reset (preserve PRIGROUP) */ + uint32_t aircr = SCB->AIRCR; + aircr &= ~SCB_AIRCR_VECTKEY_Msk; + aircr |= SCB_AIRCR_VECTKEY_WRITEMAGIC; + aircr |= SCB_AIRCR_SYSRESETREQ_Msk; + SCB->AIRCR = aircr; + + /* Wait for reset to occur */ + while (true) { + __NOP(); + } +}