From 62a439a01219ab6bad1cd73480a5d4e8d41e4248 Mon Sep 17 00:00:00 2001
From: Joel Challis <git@zvecr.com>
Date: Fri, 13 Nov 2026 08:12:39 +0000
Subject: [PATCH] Implement mcu_reset function for teensy_32/teensy_lc

---
 platforms/chibios/bootloaders/halfkay.c | 20 +++++++++++++++++++-
 1 file changed, 19 insertions(+), 1 deletion(-)

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 <ch.h>
 #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();
+    }
+}