util: Add load_vector_table

bootloader/bootloader.c

@@ -78,18 +78,7 @@ void show_unofficial_warning(const uint8_t *hash)
 
 void __attribute__((noreturn)) load_app(void)
 {
-	// Relocate vector tables
+	load_vector_table((const vector_table_t *) FLASH_APP_START);
-	SCB_VTOR = FLASH_APP_START; // & 0xFFFF;
-
-	// Set stack pointer
-	__asm__ volatile("msr msp, %0"::"g" (*(volatile uint32_t *)FLASH_APP_START));
-
-	// Jump to address
-	(*(void (**)())(FLASH_APP_START + 4))();
-
-	// forever loop to indicate to the compiler that this function does not return.
-	// this avoids the stack protector injecting code that faults with the new stack.
-	for (;;);
 }
 
 bool firmware_present(void)

firmware/fastflash.c

@@ -18,15 +18,14 @@
  */
 
 #include "fastflash.h"
+#include "util.h"
 
 #include <stdbool.h>
 #include <stdint.h>
 #include <string.h>
 
-#include <libopencm3/cm3/scb.h>
+extern uint8_t __bootloader_loadaddr__[];
-
+extern uint8_t __bootloader_runaddr__[];
-extern uint32_t __bootloader_loadaddr__[];
-extern uint32_t __bootloader_runaddr__[];
 extern uint8_t __bootloader_size__[];
 
 void load_bootloader(void)
@@ -34,19 +33,7 @@ void load_bootloader(void)
 	memcpy(__bootloader_runaddr__, __bootloader_loadaddr__, (size_t) __bootloader_size__);
 }
 
-// adapted from load_app() in bootloader.c
 void __attribute__((noreturn)) run_bootloader(void)
 {
-	// Relocate vector tables
+	load_vector_table((const vector_table_t *) __bootloader_runaddr__);
-	SCB_VTOR = (uint32_t) __bootloader_runaddr__;
-
-	// Set stack pointer
-	__asm__ volatile("msr msp, %0":: "r" (__bootloader_runaddr__[0]));
-
-	// Jump to address
-	((void (*)(void))(__bootloader_runaddr__[1]))();
-
-	// forever loop to indicate to the compiler that this function does not return.
-	// this avoids the stack protector injecting code that faults with the new stack.
-	for (;;);
 }

timer.c

@@ -22,6 +22,7 @@
 
 #include <libopencm3/stm32/rcc.h>
 #include <libopencm3/cm3/systick.h>
+#include <libopencm3/cm3/vector.h>
 
 /* 1 tick = 1 ms */
 volatile uint32_t system_millis;

timer.h

@@ -17,6 +17,8 @@
  * along with this library.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#ifndef __TIMER_H__
+#define __TIMER_H__
 
 #include <stdint.h>
 
@@ -28,4 +30,4 @@ extern uint32_t system_millis_lock_start;
 
 void timer_init(void);
 
-void sys_tick_handler(void);
+#endif

util.h

@@ -22,6 +22,9 @@
 
 #include <stdint.h>
 
+#include <libopencm3/cm3/scb.h>
+#include <libopencm3/cm3/vector.h>
+
 void delay(uint32_t wait);
 
 // converts uint32 to hexa (8 digits)
@@ -38,4 +41,18 @@ void __attribute__((noreturn)) system_halt(void);
 // reset system
 void __attribute__((noreturn)) system_reset(void);
 
+static inline void __attribute__((noreturn)) load_vector_table(const vector_table_t *vector_table) {
+    // Relocate vector table
+    SCB_VTOR = (uint32_t) vector_table;
+
+    // Set stack pointer
+    __asm__ volatile("msr msp, %0" :: "r" (vector_table->initial_sp_value));
+
+    // Jump to address
+    vector_table->reset();
+
+    // Prevent compiler from generating stack protector code (which causes CPU fault because the stack is moved)
+    for (;;);
+}
+
 #endif