From 66d7f05448d8de6524ae82d403a04528a5e6d9e7 Mon Sep 17 00:00:00 2001
From: Eike Ahmels <eike.ahmels@tu-dortmund.de>
Date: Sat, 15 Apr 2023 16:51:53 +0200
Subject: [PATCH] fix bootloader jump for g4 mcu (#12675)

---
 src/link/stm32_flash_g4_split.ld          |  2 +-
 src/main/drivers/stm32/system_stm32g4xx.c | 45 ++++++++++++++++++-----
 2 files changed, 36 insertions(+), 11 deletions(-)

diff --git a/src/link/stm32_flash_g4_split.ld b/src/link/stm32_flash_g4_split.ld
index c31f6c6fea..3d94470631 100644
--- a/src/link/stm32_flash_g4_split.ld
+++ b/src/link/stm32_flash_g4_split.ld
@@ -39,7 +39,7 @@ SECTIONS
 
   .isr_vector :
   {
-    . = ALIGN(512);
+    . = ALIGN(4);
     PROVIDE (isr_vector_table_base = .);
     KEEP(*(.isr_vector)) /* Startup code */
     . = ALIGN(4);
diff --git a/src/main/drivers/stm32/system_stm32g4xx.c b/src/main/drivers/stm32/system_stm32g4xx.c
index d9778728ec..b2e067947b 100644
--- a/src/main/drivers/stm32/system_stm32g4xx.c
+++ b/src/main/drivers/stm32/system_stm32g4xx.c
@@ -86,30 +86,55 @@ void systemReset(void)
 
 void systemResetToBootloader(bootloaderRequestType_e requestType)
 {
-    UNUSED(requestType);
+    switch (requestType) {
+    case BOOTLOADER_REQUEST_ROM:
+    default:
+        persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
 
-    persistentObjectWrite(PERSISTENT_OBJECT_RESET_REASON, RESET_BOOTLOADER_REQUEST_ROM);
+        break;
+    }
 
     __disable_irq();
     NVIC_SystemReset();
 }
 
 #define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1fff0000)
+#define SYSMEMBOOT_LOADER       ((uint32_t *)0x1fff0000)
 
 typedef void *(*bootJumpPtr)(void);
 
+typedef void resetHandler_t(void);
+
+typedef struct isrVector_s {
+    __I uint32_t    stackEnd;
+    resetHandler_t *resetHandler;
+} isrVector_t;
+
 void systemJumpToBootloader(void)
-{   
-    __SYSCFG_CLK_ENABLE();
-    
-    uint32_t bootStack =  SYSMEMBOOT_VECTOR_TABLE[0];
-    
+{
+    //DeInit all used peripherals
+    HAL_RCC_DeInit();
+
+    //Disable all system timers and set to default values
+    SysTick->CTRL = 0;
+    SysTick->LOAD = 0;
+    SysTick->VAL = 0;
+
+    //Disable all interrupts
+    __disable_irq();
+
+    //remap system memory
+    __HAL_SYSCFG_REMAPMEMORY_SYSTEMFLASH();
+
+    //default bootloader call stack routine
+    uint32_t bootStack = SYSMEMBOOT_VECTOR_TABLE[0];
+
     bootJumpPtr SysMemBootJump = (bootJumpPtr)SYSMEMBOOT_VECTOR_TABLE[1];
-    
+
     __set_MSP(bootStack); //Set the main stack pointer to its default values
-    
+
     SysMemBootJump();
-    
+
     while (1);
 }