diff --git a/Core/Src/main.c b/Core/Src/main.c
index 5d5fd36..3ddc953 100644
--- a/Core/Src/main.c
+++ b/Core/Src/main.c
@@ -87,15 +87,15 @@ volatile uint8_t pmu_irq = 0;
 static uint32_t pmu_online = 0;
 
 struct blvt_t {
-    uint32_t __sp;
-    void (*__bl_call)(void);
+    __IO uint32_t __sp;
+    __attribute((noreturn)) void (*__bl_call)(void);
 };
 #define STM32F1xxx_BL_VECTOR_TABLE	((struct blvt_t *)STM32F1xxx_BL_ADDR)
 
 
 // Private variables ---------------------------------------------------------
 //static void lock_cb(uint8_t caps_changed, uint8_t num_changed);
-static void reset_to_bootloader(void);
+static void reset_to_bootloader(uint32_t* addr);
 static void key_cb(char key, enum key_state state);
 static void hw_check_HP_presence(void);
 static void sync_bat(void);
@@ -293,6 +293,8 @@ int main(void) {
 
 	LL_GPIO_ResetOutputPin(SYS_LED_GPIO_Port, SYS_LED_Pin);	// I'm alive!
 
+	reset_to_bootloader((uint32_t*)STM32F1xxx_BL_ADDR);
+
 	// Start the systick timer
 	if (HAL_TIM_Base_Start_IT(&htim2) != HAL_OK)
 		Error_Handler();
@@ -481,36 +483,63 @@ static void lock_cb(uint8_t caps_changed, uint8_t num_changed) {
 */
 
 
+__attribute__((naked, noreturn)) static void BootJumpASM(
+		__attribute__((unused)) uint32_t __SP, 
+		__attribute__((unused)) uint32_t __RH )
+{
+	__asm("MSR      MSP,r0");
+	__asm("BX       r1");
+}
 
-static void reset_to_bootloader(void) {
+static void reset_to_bootloader(uint32_t* addr) {
 	// Disable IRQ to not break everything
 	__disable_irq();
 
 	// Reset the system to a boot state
-	HAL_RCC_DeInit();
+	//HAL_RCC_DeInit();
 	SysTick->CTRL = 0;
 	SysTick->LOAD = 0;
 	SysTick->VAL = 0;
+	SCB->ICSR |= SCB_ICSR_PENDSTCLR_Msk ;
+
+	// Disable individual fault handlers if the bootloader used them
+	SCB->SHCSR &= ~( SCB_SHCSR_USGFAULTENA_Msk | \
+					 SCB_SHCSR_BUSFAULTENA_Msk | \
+					 SCB_SHCSR_MEMFAULTENA_Msk ) ;
 
 	for (uint8_t i = 0; i < sizeof(NVIC->ICER) / sizeof(NVIC->ICER[0]); i++) {
 		NVIC->ICER[i]=0xFFFFFFFF;
 		NVIC->ICPR[i]=0xFFFFFFFF;
 	}
 
-	__enable_irq();
+	//__enable_irq();
 
-	// Relocate the main SP
-	__set_MSP(STM32F1xxx_BL_VECTOR_TABLE->__sp);
-
-	// Remap NVIC
-	//SCB->VTOR = STM32F1xxx_BL_ADDR;
+	// Activate the MSP, if the core is found to currently run with the PSP.
+	if( CONTROL_SPSEL_Msk & __get_CONTROL( ) )
+	{  /* MSP is not active */
+	  __set_CONTROL( __get_CONTROL( ) & ~CONTROL_SPSEL_Msk ) ;
+	}
 
 	// Wait for every datas to be ready
 	__ISB();
 	__DSB();
 
+	// Remap the vector table address of the bootloader. Make sure the address meets the alignment requirements.
+	SCB->VTOR = (uint32_t)addr;
+
+	// Wait for every datas to be ready
+	__ISB();
+	__DSB();
+
+	// Relocate the main SP
+	__set_MSP( addr[0] ) ;
+	//__set_MSP(STM32F1xxx_BL_VECTOR_TABLE->__sp);
+
 	// Now, jump to the bootloader!
-	STM32F1xxx_BL_VECTOR_TABLE->__bl_call();
+	( ( void ( * )( void ) )addr[1] )( ) ;
+	//STM32F1xxx_BL_VECTOR_TABLE->__bl_call();
+
+	//BootJumpASM(((uint32_t*)STM32F1xxx_BL_ADDR)[0], ((uint32_t*)STM32F1xxx_BL_ADDR)[1]);
 
 	for (;;)
 		// We should never go here...
@@ -795,7 +824,7 @@ __STATIC_INLINE void check_pmu_int(void) {
 			//uint8_t data[4] = {1, 2, 3, 4};
 			//PMU.writeDataBuffer(data, XPOWERS_AXP2101_DATA_BUFFER_SIZE);
 
-			reset_to_bootloader();
+			reset_to_bootloader((uint32_t*)STM32F1xxx_BL_ADDR);
 
 			/*LL_GPIO_ResetOutputPin(SP_AMP_EN_GPIO_Port, SP_AMP_EN_Pin);
 			LL_GPIO_ResetOutputPin(PICO_EN_GPIO_Port, PICO_EN_Pin);