From 7b895a2561367f3f823a77348b0af80821a6ccf1 Mon Sep 17 00:00:00 2001
From: Markus Koch <markus@notsyncing.net>
Date: Tue, 2 Mar 2021 19:27:50 +0100
Subject: [PATCH] Add jump-to-bootloader functionality

---
 firmware/lib/systems.c | 26 ++++++++++++++++++++++++++
 firmware/lib/systems.h |  4 +++-
 2 files changed, 29 insertions(+), 1 deletion(-)

diff --git a/firmware/lib/systems.c b/firmware/lib/systems.c
index 513b916..a7390f0 100644
--- a/firmware/lib/systems.c
+++ b/firmware/lib/systems.c
@@ -262,3 +262,29 @@ void ConfigureGPIO( uint8_t gpio, int parameters )
 	GPIO_Init(GPIOOf(gpio), &GPIO_InitStructure);
 }
 
+void JumpToBootloader(void) {
+	// Many thanks to https://stm32f4-discovery.net/2017/04/tutorial-jump-system-memory-software-stm32/
+
+	void (*SysMemBootJump)(void);
+	volatile uint32_t addr = 0x1FFF0000;
+
+	RCC_DeInit();
+
+	// Disable and reset systick
+	SysTick->CTRL = 0;
+	SysTick->LOAD = 0;
+	SysTick->VAL = 0;
+
+	__disable_irq();
+
+	// Remap system memory	
+	SYSCFG->MEMRMP = 0x01;
+
+	SysMemBootJump = (void (*)(void)) (*((uint32_t *)(addr + 4)));
+
+	// Set main stack pointer
+	__set_MSP(*(uint32_t *)addr);
+
+	// Jump
+	SysMemBootJump();
+}
diff --git a/firmware/lib/systems.h b/firmware/lib/systems.h
index bb0fa10..3c17f6c 100644
--- a/firmware/lib/systems.h
+++ b/firmware/lib/systems.h
@@ -54,8 +54,10 @@ void ConfigureGPIO( gpio gpio, int parameters );
 #endif
 
 void ConfigureUART();
-
 void ConfigureLED();
+
+void JumpToBootloader(void);
+
 #define LED_TOGGLE {GPIOOf(LEDPIN)->ODR^=(1<<((LEDPIN)&0x0f));}
 #define LED_ON     GPIOOn(LEDPIN)
 #define LED_OFF    GPIOOff(LEDPIN)