From 8d3020bc624f43d63fb103edc9b16f1618b33027 Mon Sep 17 00:00:00 2001
From: Pavol Rusnak <stick@gk2.sk>
Date: Wed, 29 Mar 2017 20:50:45 +0200
Subject: [PATCH] trezorhal: implement jump_to function

---
 micropython/bootloader/main.c  | 3 +--
 micropython/loader/main.c      | 6 +++++-
 micropython/trezorhal/common.c | 7 +++++++
 micropython/trezorhal/common.h | 4 ++++
 4 files changed, 17 insertions(+), 3 deletions(-)

diff --git a/micropython/bootloader/main.c b/micropython/bootloader/main.c
index 6a0e5f822..243580b6d 100644
--- a/micropython/bootloader/main.c
+++ b/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");
         }
diff --git a/micropython/loader/main.c b/micropython/loader/main.c
index 0141e20f3..7bf607379 100644
--- a/micropython/loader/main.c
+++ b/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;
 }
diff --git a/micropython/trezorhal/common.c b/micropython/trezorhal/common.c
index ec9cbd9c1..d17d8351f 100644
--- a/micropython/trezorhal/common.c
+++ b/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))();
+}
diff --git a/micropython/trezorhal/common.h b/micropython/trezorhal/common.h
index 8df23bb4f..f8de1fafc 100644
--- a/micropython/trezorhal/common.h
+++ b/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