trezorhal: implement jump_to function

micropython/bootloader/main.c

@@ -140,8 +140,7 @@ int main(void)
         if (check_signature((const uint8_t *)LOADER_START)) {
             BOOTLOADER_PRINTLN("valid loader signature");
             BOOTLOADER_PRINTLN("JUMP!");
-            // TODO: jump to loader
-            __fatal_error("halt");
+            jump_to(LOADER_START);
         } else {
             BOOTLOADER_PRINTLN("invalid loader signature");
         }

micropython/loader/main.c

@@ -23,7 +23,11 @@ int main(void)
     display_clear();
     display_backlight(255);
 
-    __fatal_error("end reached");
+    LOADER_PRINTLN("reached loader");
+    HAL_Delay(1000);
+    LOADER_PRINTLN("jumping to firmware");
+
+    jump_to(FIRMWARE_START);
 
     return 0;
 }

micropython/trezorhal/common.c

@@ -56,3 +56,10 @@ void periph_init(void) {
     DWT->CYCCNT = 0;  // Reset Cycle Count Register
     DWT->CTRL |= DWT_CTRL_CYCCNTENA_Msk;  // Enable Cycle Count Register
 }
+
+void jump_to(uint32_t start)
+{
+    SCB->VTOR = start;
+    __asm__ volatile("msr msp, %0"::"g" (*(volatile uint32_t *)start));
+    (*(void (**)())(start + 4))();
+}

micropython/trezorhal/common.h

@@ -1,10 +1,14 @@
 #ifndef __TREZORHAL_COMMON_H__
 #define __TREZORHAL_COMMON_H__
 
+#include <stdint.h>
+
 void periph_init(void);
 
 void __attribute__((noreturn)) nlr_jump_fail(void *val);
 
 void __attribute__((noreturn)) __fatal_error(const char *msg);
 
+void jump_to(uint32_t address);
+
 #endif