From 65aa3b49d9eb5f4e34406f643c7b28355615a8bb Mon Sep 17 00:00:00 2001
From: Ondrej Mikle <ondrej.mikle@gmail.com>
Date: Tue, 9 Feb 2021 08:29:36 +0100
Subject: [PATCH] feat(legacy): add supervisor call for reboot to bootloader

---
 legacy/supervise.c | 9 +++++++++
 legacy/supervise.h | 9 +++++++++
 2 files changed, 18 insertions(+)

diff --git a/legacy/supervise.c b/legacy/supervise.c
index 9395051f8b..76e175d4c6 100644
--- a/legacy/supervise.c
+++ b/legacy/supervise.c
@@ -19,6 +19,7 @@
 
 #include "supervise.h"
 #include <libopencm3/stm32/flash.h>
+#include <vendor/libopencm3/include/libopencmsis/core_cm3.h>
 #include <stdint.h>
 #include "memory.h"
 
@@ -62,6 +63,11 @@ static uint32_t svhandler_flash_lock(void) {
   return FLASH_SR;
 }
 
+static void __attribute__((noreturn)) svhandler_reboot_to_bootloader(void) {
+  __asm__ __volatile__("ldr r12, =0x55aa55aa");
+  scb_reset_system();
+}
+
 extern volatile uint32_t system_millis;
 
 void svc_handler_main(uint32_t *stack) {
@@ -82,6 +88,9 @@ void svc_handler_main(uint32_t *stack) {
     case SVC_TIMER_MS:
       stack[0] = system_millis;
       break;
+    case SVC_REBOOT_TO_BOOTLOADER:
+      svhandler_reboot_to_bootloader();
+      break;
     default:
       stack[0] = 0xffffffff;
       break;
diff --git a/legacy/supervise.h b/legacy/supervise.h
index 4ff216197f..868e01e55c 100644
--- a/legacy/supervise.h
+++ b/legacy/supervise.h
@@ -29,6 +29,7 @@
 #define SVC_FLASH_PROGRAM 2
 #define SVC_FLASH_LOCK 3
 #define SVC_TIMER_MS 4
+#define SVC_REBOOT_TO_BOOTLOADER 5
 
 /* Unlocks flash.  This function needs to be called before programming
  * or erasing. Multiple calls of flash_program and flash_erase can
@@ -71,6 +72,14 @@ inline uint32_t svc_timer_ms(void) {
   return r0;
 }
 
+/* Reboot to bootloader.
+ * Sets a flag in register and reboots. When bootloader runs, it will recognize
+ * flag and won't run firmware, but enter bootloader mode.
+ */
+inline void svc_reboot_to_bootloader(void) {
+    __asm__ __volatile__("svc %0" ::"i"(SVC_REBOOT_TO_BOOTLOADER) : "memory");
+}
+
 #else
 
 extern void svc_flash_unlock(void);