fix(core): fix handover to bootloader

[no changelog]

core/embed/trezorhal/stm32f4/bootutils.c

@@ -82,6 +82,65 @@ static inline void __attribute__((always_inline)) delete_secrets(void) {
 }
 #endif  // STM32U5
 
+#ifdef STM32F4
+// Ensure that we are running in privileged thread mode.
+//
+// This function is used only on STM32F4, where a direct jump to the
+// bootloader is performed. It checks if we are in handler mode, and
+// if so, it switches to privileged thread mode.
+__attribute((naked, no_stack_protector)) static void ensure_thread_mode(void) {
+  __asm__ volatile(
+      // --------------------------------------------------------------
+      // Check if we are in handler mode
+      // --------------------------------------------------------------
+
+      "LDR      R1, =0x1FF         \n"  // Get lower 9 bits of IPSR
+      "MRS      R0, IPSR           \n"
+      "ANDS     R0, R0, R1         \n"
+      "CMP      R0, #0             \n"  // == 0 if in thread mode
+      "IT       EQ                 \n"
+      "BXEQ     LR                 \n"  // return if in thread mode
+
+      // --------------------------------------------------------------
+      // Disable FP registers lazy stacking
+      // --------------------------------------------------------------
+
+      "LDR     R1, = 0xE000EF34    \n"  // FPU->FPCCR
+      "LDR     R0, [R1]            \n"
+      "BIC     R0, R0, #1          \n"  // Clear LSPACT to suppress lazy
+                                        // stacking
+      "STR     R0, [R1]            \n"
+
+      // --------------------------------------------------------------
+      // Exit handler mode, enter thread mode
+      // --------------------------------------------------------------
+
+      "MOV     R0, SP              \n"  // Align stack pointer to 8 bytes
+      "AND     R0, R0, #~7         \n"
+      "MOV     SP, R0              \n"
+      "SUB     SP, SP, #32         \n"  // Allocate space for the stack frame
+
+      "MOV     R0, #0              \n"
+      "STR     R0, [SP, #0]        \n"  // future R0 = 0
+      "STR     R0, [SP, #4]        \n"  // future R1 = 0
+      "STR     R0, [SP, #8]        \n"  // future R2 = 0
+      "STR     R0, [SP, #12]       \n"  // future R3 = 0
+      "STR     R12, [SP, #16]      \n"  // future R12 = R12
+      "STR     LR, [SP, #20]       \n"  // future LR = LR
+      "BIC     LR, LR, #1          \n"
+      "STR     LR, [SP, #24]       \n"  // return address = LR
+      "LDR     R0, = 0x01000000    \n"  // THUMB bit set
+      "STR     R0, [SP, #28]       \n"  // future xPSR
+
+      "MRS     R0, CONTROL         \n"  // Clear SPSEL to use MSP for thread
+      "BIC     R0, R0, #3          \n"  // Clear nPRIV to run in privileged mode
+      "MSR     CONTROL, R0         \n"
+
+      "LDR     LR, = 0xFFFFFFF9    \n"  // Return to Secure Thread mode, use MSP
+      "BX      LR                  \n");
+}
+#endif  // STM32F4
+
 // Reboots the device with the given boot command and arguments
 static void __attribute__((noreturn))
 reboot_with_args(boot_command_t command, const void* args, size_t args_size) {
@@ -98,6 +157,8 @@ reboot_with_args(boot_command_t command, const void* args, size_t args_size) {
 
   mpu_reconfig(MPU_MODE_DISABLED);
 
+  ensure_thread_mode();
+
   jump_to_with_flag(BOOTLOADER_START + IMAGE_HEADER_SIZE, g_boot_command);
   for (;;)
     ;