diff --git a/src/main/drivers/system_stm32h7xx.c b/src/main/drivers/system_stm32h7xx.c
index ff50958418..11e2f070f6 100644
--- a/src/main/drivers/system_stm32h7xx.c
+++ b/src/main/drivers/system_stm32h7xx.c
@@ -128,6 +128,27 @@ void systemResetToBootloader(bootloaderRequestType_e requestType)
     NVIC_SystemReset();
 }
 
+
+#define SYSMEMBOOT_VECTOR_TABLE ((uint32_t *)0x1ff09800)
+
+typedef void *(*bootJumpPtr)(void);
+
+void systemJumpToBootloader(void)
+{
+    __SYSCFG_CLK_ENABLE();
+
+    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);
+}
+
+
 static uint32_t bootloaderRequest;
 
 void systemCheckResetReason(void)
@@ -160,12 +181,5 @@ void systemCheckResetReason(void)
         forcedSystemResetWithoutDisablingCaches(); // observed that disabling dcache after cold boot with BOOT pin high causes segfault.
     }
 
-    void (*SysMemBootJump)(void);
-    __SYSCFG_CLK_ENABLE();
-
-    uint32_t p =  (*((uint32_t *) 0x1ff09800));
-    __set_MSP(p); //Set the main stack pointer to its defualt values
-    SysMemBootJump = (void (*)(void)) (*((uint32_t *) 0x1ff09804)); // Point the PC to the System Memory reset vector (+4)
-    SysMemBootJump();
-    while (1);
+    systemJumpToBootloader();
 }