From 2697c06642b9b1711d8d375fcafcec3c38032d4b Mon Sep 17 00:00:00 2001 From: cepetr Date: Wed, 29 Jan 2025 21:34:20 +0100 Subject: [PATCH] refactor(core): rewrite rescue code in C [no changelog] --- core/embed/projects/firmware/main.c | 4 - core/embed/sys/linker/inc/sys/linker_utils.h | 4 + core/embed/sys/linker/linker_utils.c | 2 - core/embed/sys/startup/inc/sys/bootargs.h | 16 +- core/embed/sys/startup/inc/sys/sysutils.h | 46 ++++ core/embed/sys/startup/stm32/bootutils.c | 62 +---- core/embed/sys/startup/stm32/sysutils.c | 215 ++++++++++++++++++ core/embed/sys/startup/stm32f4/startup_init.c | 2 + core/embed/sys/startup/stm32u5/startup_init.c | 1 - core/embed/sys/task/stm32/systask.c | 5 +- core/embed/sys/task/stm32/system.c | 201 ++++------------ core/site_scons/models/stm32f4_common.py | 1 + core/site_scons/models/stm32u5_common.py | 1 + 13 files changed, 333 insertions(+), 227 deletions(-) create mode 100644 core/embed/sys/startup/inc/sys/sysutils.h create mode 100644 core/embed/sys/startup/stm32/sysutils.c diff --git a/core/embed/projects/firmware/main.c b/core/embed/projects/firmware/main.c index 5995091056..3943589450 100644 --- a/core/embed/projects/firmware/main.c +++ b/core/embed/projects/firmware/main.c @@ -42,10 +42,6 @@ #include "zkp_context.h" #endif -// symbols defined in the linker script -extern uint8_t _stack_section_start; -extern uint8_t _stack_section_end; - int main(uint32_t cmd, void *arg) { if (cmd == 1) { systask_postmortem_t *info = (systask_postmortem_t *)arg; diff --git a/core/embed/sys/linker/inc/sys/linker_utils.h b/core/embed/sys/linker/inc/sys/linker_utils.h index e8307d38be..9975fc8372 100644 --- a/core/embed/sys/linker/inc/sys/linker_utils.h +++ b/core/embed/sys/linker/inc/sys/linker_utils.h @@ -20,6 +20,10 @@ #pragma once +// symbols defined in the linker script +extern uint8_t _stack_section_start; +extern uint8_t _stack_section_end; + // Initialize linker script-defined sections (.bss, .data, etc.) // // This function must be called only during the startup sequence, diff --git a/core/embed/sys/linker/linker_utils.c b/core/embed/sys/linker/linker_utils.c index 47bf18b7ca..84419ed12b 100644 --- a/core/embed/sys/linker/linker_utils.c +++ b/core/embed/sys/linker/linker_utils.c @@ -55,8 +55,6 @@ void init_linker_sections(void) { } __attribute((naked, no_stack_protector)) void clear_unused_stack(void) { - extern uint32_t _stack_section_start; - __asm__ volatile( " MOV R0, #0 \n" " LDR R1, =%[sstack] \n" diff --git a/core/embed/sys/startup/inc/sys/bootargs.h b/core/embed/sys/startup/inc/sys/bootargs.h index f566c5cbca..051040404c 100644 --- a/core/embed/sys/startup/inc/sys/bootargs.h +++ b/core/embed/sys/startup/inc/sys/bootargs.h @@ -20,6 +20,7 @@ #ifndef TREZORHAL_BOOTARGS_H #define TREZORHAL_BOOTARGS_H +#include #include // Defines boot command processed in bootloader on next reboot @@ -30,6 +31,8 @@ typedef enum { BOOT_COMMAND_STOP_AND_WAIT = 0x0FC35A96, // Do not ask anything, install an upgrade BOOT_COMMAND_INSTALL_UPGRADE = 0xFA4A5C8D, + // Show RSOD and wait for user input + BOOT_COMMAND_SHOW_RSOD = 0x7CD945A0, } boot_command_t; // Maximum size boot_args array @@ -37,12 +40,15 @@ typedef enum { typedef union { uint8_t raw[BOOT_ARGS_MAX_SIZE]; - // firmware header hash, BOOT_COMMAND_INSTALL_UPGRADE uint8_t hash[32]; - + // error information, BOOT_COMMAND_SHOW_RSOD + systask_postmortem_t pminfo; } boot_args_t; +_Static_assert(sizeof(boot_args_t) == BOOT_ARGS_MAX_SIZE, + "boot_args_t structure is too long"); + // Initialize bootargs module after bootloader startup // // r11_register is the value of the r11 register at bootloader entry. @@ -59,4 +65,10 @@ boot_command_t bootargs_get_command(); // Copies the boot arguments to the destination buffer void bootargs_get_args(boot_args_t* dest); +// Returns a pointer to the boot arguments structure. +// +// This function is intended to be used only in rescue mode, when the MPU +// is disabled and the caller has full access to the boot arguments area. +boot_args_t* bootargs_ptr(void); + #endif // TREZORHAL_BOOTARGS_H diff --git a/core/embed/sys/startup/inc/sys/sysutils.h b/core/embed/sys/startup/inc/sys/sysutils.h new file mode 100644 index 0000000000..c38615b71e --- /dev/null +++ b/core/embed/sys/startup/inc/sys/sysutils.h @@ -0,0 +1,46 @@ +/* + * This file is part of the Trezor project, https://trezor.io/ + * + * Copyright (c) SatoshiLabs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#pragma once + +#ifdef KERNEL_MODE + +#include + +typedef void (*new_stack_callback_t)(uint32_t arg1, uint32_t arg2); + +// Disables interrupts, disables the MPU, clears +// all registers, sets up a new stack and calls the given callback. +// +// The function is intended to be used in special cases, like +// emergency situations, where the current stack may be corrupted. +__attribute((noreturn)) void call_with_new_stack(uint32_t arg1, uint32_t arg2, + new_stack_callback_t callback); + +// 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. +void ensure_thread_mode(void); + +// Clears USB peripheral fifo memory +void clear_otg_hs_memory(void); + +#endif // KERNEL_MODE diff --git a/core/embed/sys/startup/stm32/bootutils.c b/core/embed/sys/startup/stm32/bootutils.c index a0c4858e37..3cde2ced9c 100644 --- a/core/embed/sys/startup/stm32/bootutils.c +++ b/core/embed/sys/startup/stm32/bootutils.c @@ -26,6 +26,7 @@ #include #include #include +#include #include #ifdef TREZOR_MODEL_T2T1 @@ -68,6 +69,8 @@ void bootargs_set(boot_command_t command, const void* args, size_t args_size) { mpu_restore(mode); } +boot_args_t* bootargs_ptr(void) { return &g_boot_args; } + #ifdef BOOTLOADER // Contains the current boot command saved during bootloader startup. boot_command_t g_boot_command_saved; @@ -105,65 +108,6 @@ 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) { diff --git a/core/embed/sys/startup/stm32/sysutils.c b/core/embed/sys/startup/stm32/sysutils.c new file mode 100644 index 0000000000..334a43a462 --- /dev/null +++ b/core/embed/sys/startup/stm32/sysutils.c @@ -0,0 +1,215 @@ +/* + * This file is part of the Trezor project, https://trezor.io/ + * + * Copyright (c) SatoshiLabs + * + * This program is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see . + */ + +#include + +#include +#include + +#ifdef KERNEL_MODE + +__attribute((naked, noreturn, no_stack_protector)) void call_with_new_stack( + uint32_t arg1, uint32_t arg2, new_stack_callback_t callback) { + __asm__ volatile( + + // R0, R1, R2 are used for arguments + + "CPSID F \n" // Disable interrupts/faults + + // -------------------------------------------------------------- + // Disable MPU + // -------------------------------------------------------------- + + "DMB 0xF \n" // Data memory barrier + "LDR R4, =0xE000ED94 \n" // MPU->CTRL + "MOV R5, #0 \n" + "STR R5, [R4] \n" // Disable MPU + ); + +#ifdef STM32U5 + __asm__ volatile( + // -------------------------------------------------------------- + // Delete all secrets and SRAM2 where stack is located. + // SAES peripheral need to be disabled, so that we don't get + // tamper events. + // -------------------------------------------------------------- + + // RCC->AHBENR1 &= ~RCC_AHBENR1_SAESEN; + "LDR R4, =%[_RCC_AHB2ENR1] \n" + "LDR R5, =%[_RCC_AHB2ENR1_SAESEN] \n" + "LDR R6, [R4] \n" + "BIC R6, R6, R5 \n" + "STR R6, [R4] \n" + + // TAMP->CR2 |= TAMP_CR2_BKERASE; + "LDR R4, =%[_TAMP_CR2] \n" + "LDR R5, =%[_TAMP_CR2_BKERASE] \n" + "LDR R6, [R4] \n" + "ORR R6, R6, R5 \n" + "STR R6, [R4] \n" + + : // no output + : [_RCC_AHB2ENR1] "i"(&RCC->AHB2ENR1), + [_RCC_AHB2ENR1_SAESEN] "i"(RCC_AHB2ENR1_SAESEN), + [_TAMP_CR2] "i"(&TAMP->CR2), + [_TAMP_CR2_BKERASE] "i"(TAMP_CR2_BKERASE) + + : // no clobber + ); +#endif // STM32U5 + + __asm__ volatile( + // -------------------------------------------------------------- + // Setup new stack + // -------------------------------------------------------------- + + "LDR R4, =%[estack] \n" // Setup new stack + "MSR MSP, R4 \n" // Set MSP +#if defined(__ARM_ARCH_8M_MAIN__) || defined(__ARM_ARCH_8M_BASE__) + "LDR R4, =%[sstack] \n" + "ADD R4, R4, #256 \n" // Add safety margin + "MSR MSPLIM, R4 \n" // Set MSPLIM +#endif + + // -------------------------------------------------------------- + // Clear all VFP registers + // -------------------------------------------------------------- + + "LDR R4, = 0xE000EF34 \n" // FPU->FPCCR + "LDR R5, [R4] \n" + "BIC R5, R5, #1 \n" // Clear LSPACT to suppress lazy + // stacking + "STR R5, [R4] \n" + + // TODO: clear VFP registers (maybe for ARMV7-M only) + + // -------------------------------------------------------------- + // Clear all unused registers + // -------------------------------------------------------------- + + "MOV R3, #0 \n" + "MOV R4, R3 \n" + "MOV R5, R3 \n" + "MOV R6, R3 \n" + "MOV R7, R3 \n" + "MOV R8, R3 \n" + "MOV R9, R3 \n" + "MOV R10, R3 \n" + "MOV R11, R3 \n" + "MOV R12, R3 \n" + + // -------------------------------------------------------------- + // Invoke phase 2 function + // -------------------------------------------------------------- + + // R0 = arg1 + // R1 = arg2 + + "BX R2 \n" + + : // no output + : [estack] "i"(&_stack_section_end), + [sstack] "i"(&_stack_section_start) + : // no clobber + ); +} + +// 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)) 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"); +} + + +// Clears USB FIFO memory to prevent data leakage of sensitive information +__attribute((used)) void clear_otg_hs_memory(void) { +#ifdef STM32F4 + + // reference RM0090 section 35.12.1 Figure 413 + #define USB_OTG_HS_DATA_FIFO_RAM (USB_OTG_HS_PERIPH_BASE + 0x20000U) + #define USB_OTG_HS_DATA_FIFO_SIZE (4096U) + + // use the HAL version due to section 2.1.6 of STM32F42xx Errata sheet + __HAL_RCC_USB_OTG_HS_CLK_ENABLE(); // enable USB_OTG_HS peripheral clock so + // that the peripheral memory is + // accessible + __IO uint32_t* usb_fifo_ram = (__IO uint32_t*)USB_OTG_HS_DATA_FIFO_RAM; + + for (uint32_t i = 0; i < USB_OTG_HS_DATA_FIFO_SIZE / 4; i++) { + usb_fifo_ram[i] = 0; + } + + __HAL_RCC_USB_OTG_HS_CLK_DISABLE(); // disable USB OTG_HS peripheral clock as + // the peripheral is not needed right now +#endif +} + + +#endif // KERNEL_MODE diff --git a/core/embed/sys/startup/stm32f4/startup_init.c b/core/embed/sys/startup/stm32f4/startup_init.c index f6f535c710..040f87a857 100644 --- a/core/embed/sys/startup/stm32f4/startup_init.c +++ b/core/embed/sys/startup/stm32f4/startup_init.c @@ -26,6 +26,8 @@ #include #include #include +#include + #include "startup_init.h" #ifdef KERNEL_MODE diff --git a/core/embed/sys/startup/stm32u5/startup_init.c b/core/embed/sys/startup/stm32u5/startup_init.c index ca4f4af923..524c8baffc 100644 --- a/core/embed/sys/startup/stm32u5/startup_init.c +++ b/core/embed/sys/startup/stm32u5/startup_init.c @@ -222,7 +222,6 @@ void SystemInit(void) { __attribute((no_stack_protector)) void reset_handler(void) { // Set stack pointer limit for checking stack overflow - extern uint8_t _stack_section_start; __set_MSPLIM((uintptr_t)&_stack_section_start + 128); // Now .bss, .data are not initialized yet - we need to be diff --git a/core/embed/sys/task/stm32/systask.c b/core/embed/sys/task/stm32/systask.c index 2c459b67e6..7e7be92f8c 100644 --- a/core/embed/sys/task/stm32/systask.c +++ b/core/embed/sys/task/stm32/systask.c @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -54,10 +55,6 @@ typedef struct { systask_t* waiting_task; } systask_scheduler_t; -// Kernel stack base pointer defined in linker script -extern uint8_t _stack_section_start; -extern uint8_t _stack_section_end; - // Global task manager state static systask_scheduler_t g_systask_scheduler = { // This static initialization is required for exception handling diff --git a/core/embed/sys/task/stm32/system.c b/core/embed/sys/task/stm32/system.c index 1231a4d2aa..bb8f2d4c45 100644 --- a/core/embed/sys/task/stm32/system.c +++ b/core/embed/sys/task/stm32/system.c @@ -20,12 +20,15 @@ #include #include +#include #include +#include #include #include #include #include #include +#include #ifdef USE_SDRAM #include @@ -41,10 +44,6 @@ #ifdef KERNEL_MODE -// Kernel stack base pointer defined in linker script -extern uint8_t _stack_section_start; -extern uint8_t _stack_section_end; - void system_init(systask_error_handler_t error_handler) { #if defined(TREZOR_MODEL_T2T1) && (!defined(BOARDLOADER)) // Early boardloader versions on Model T initialized the CPU clock to 168MHz. @@ -179,168 +178,60 @@ __attribute__((used)) static void emergency_reset(void) { SCB->SHCSR &= ~(cleared_flags & ~preserved_flag); } -__attribute((naked, no_stack_protector)) void system_emergency_rescue( - systask_error_handler_t error_handler, const systask_postmortem_t* pminfo) { +__attribute((noreturn, no_stack_protector)) static void +system_emergency_rescue_phase_2(uint32_t arg1, uint32_t arg2) { + systask_error_handler_t error_handler = (systask_error_handler_t)arg1; + + // Reset peripherals (so we are sure that no DMA is pending) + emergency_reset(); + + // Copy pminfo from to our stack + // MPU is now disable, we have full access to bootargs. + systask_postmortem_t pminfo = bootargs_ptr()->pminfo; + + // Clear unused part of our stack + clear_unused_stack(); + + // Save stack protector guard for later extern uint32_t __stack_chk_guard; + uint32_t stack_chk_guard = __stack_chk_guard; - __asm__ volatile( - "MOV R5, R1 \n" // R5 = pminfo - "MOV R6, R0 \n" // R6 = error_handler + // Clear all memory except our stack. + // NOTE: This also clear bootargs, so we don't pass pminfo structure + // to the bootloader for now. + memregion_t region = MEMREGION_ALL_ACCESSIBLE_RAM; + MEMREGION_DEL_SECTION(®ion, _stack_section); + memregion_fill(®ion, 0); - "CPSID I \n" // Disable interrupts + // Reinitialize .bss, .data, ... + init_linker_sections(); - // -------------------------------------------------------------- - // Disable MPU - // -------------------------------------------------------------- + // Reinitialize stack protector guard + __stack_chk_guard = stack_chk_guard; - "DMB 0xF \n" // Data memory barrier - "LDR R0, =0xE000ED94 \n" // MPU->CTRL - "MOV R1, #0 \n" - "STR R1, [R0] \n" // Disable MPU + // Now we can safely enable interrupts again + __enable_fault_irq(); - // -------------------------------------------------------------- - // Setup new stack - // -------------------------------------------------------------- + // Ensure we are in thread mode + ensure_thread_mode(); - "LDR R0, =%[estack] \n" // Setup new stack - "MSR MSP, R0 \n" // Set MSP -#if defined(__ARM_ARCH_8M_MAIN__) || defined(__ARM_ARCH_8M_BASE__) - "LDR R0, =%[sstack] \n" - "ADD R0, R0, #256 \n" // Add safety margin - "MSR MSPLIM, R0 \n" // Set MSPLIM -#endif + // Now everything is perfectly initialized and we can do anything + // in C code - // -------------------------------------------------------------- - // Copy pminfo to new stack - // -------------------------------------------------------------- + if (error_handler != NULL) { + error_handler(&pminfo); + } - "LDR R2, =%[PMINFO_SIZE] \n" // Copy pminfo to new stack - "SUB SP, R2 \n" // Allocate space for pminfo - "MOV R0, SP \n" // Destination - "MOV R1, R5 \n" // Source - "MOV R5, R0 \n" // R5 = pminfo on the new stack - "BL memcpy \n" + secure_shutdown(); +} - // -------------------------------------------------------------- - // Save stack protector guard - // -------------------------------------------------------------- +__attribute((naked, noreturn, no_stack_protector)) void system_emergency_rescue( + systask_error_handler_t error_handler, const systask_postmortem_t* pminfo) { + // Save `pminfo` to bootargs so it isn't overwritten by succesive call + bootargs_set(BOOT_COMMAND_SHOW_RSOD, pminfo, sizeof(*pminfo)); - "LDR R0, =%[STK_GUARD] \n" // Save stack protector guard - "LDR R7, [R0] \n" // R7 = __stack_chk_guard - - // -------------------------------------------------------------- - // Clear .bss, initialize .data, ... - // -------------------------------------------------------------- - - "LDR R0, =_bss_section_start \n" // Clear .bss - "MOV R1, #0 \n" - "LDR R2, =_bss_section_end \n" - "SUB R2, R2, R0 \n" - "BL memset \n" - - "LDR R0, =_data_section_start \n" // Initialize .data - "LDR R1, =_data_section_loadaddr \n" - "LDR R2, =_data_section_end \n" - "SUB R2, R2, R0 \n" - "BL memcpy \n" - -#ifdef STM32U5 - "LDR R0, =_confidential_section_start \n" // Initialize .confidental - "LDR R1, =_confidential_section_loadaddr \n" - "LDR R2, =_confidential_section_end \n" - "SUB R2, R2, R0 \n" - "BL memcpy \n" -#endif - - // -------------------------------------------------------------- - // Restore stack protector guard - // -------------------------------------------------------------- - - "LDR R0, =%[STK_GUARD] \n" // Restore stack protector guard - "STR R7, [R0] \n" - - // -------------------------------------------------------------- - // Reset critical hardware so we can safely enable interrupts - // -------------------------------------------------------------- - - "BL emergency_reset \n" - - "CPSIE I \n" // Re-enable interrupts - - // -------------------------------------------------------------- - // Clear all VFP registers - // -------------------------------------------------------------- - - "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" - - // TODO: clear VFP registers (maybe for ARMV7-M only) - - // -------------------------------------------------------------- - // Clear R7-R11 registers - // -------------------------------------------------------------- - - "MOV R0, #0 \n" - "MOV R7, R0 \n" - "MOV R8, R0 \n" - "MOV R9, R0 \n" - "MOV R10, R0 \n" - "MOV R11, R0 \n" - - // -------------------------------------------------------------- - // Check if we are in thread mode and if yes, jump to error_handler - // -------------------------------------------------------------- - - "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 - "ITTT EQ \n" - "MOVEQ R0, R5 \n" // R0 = pminfo - "LDREQ LR, =secure_shutdown\n" - "BXEQ R6 \n" // jump to error_handler directly - - // -------------------------------------------------------------- - // Return from exception to 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 R5, [SP, #0] \n" // future R0 = pminfo - "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 R0, [SP, #16] \n" // future R12 = 0 - "LDR R0, =secure_shutdown\n" - "STR R0, [SP, #20] \n" // future LR = secure_shutdown() - "BIC R6, R6, #1 \n" - "STR R6, [SP, #24] \n" // return address = error_handler() - "LDR R1, = 0x01000000 \n" // THUMB bit set - "STR R1, [SP, #28] \n" // future xPSR - - "MOV R4, R0 \n" // Clear registers R4-R6 - "MOV R5, R0 \n" // (R7-R11 are already cleared) - "MOV R6, R0 \n" - - "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, R1 \n" - - "LDR LR, = 0xFFFFFFF9 \n" // Return to Secure Thread mode, use MSP - "BX LR \n" - : // no output - : [PMINFO_SIZE] "i"(sizeof(systask_postmortem_t)), - [STK_GUARD] "i"(&__stack_chk_guard), [estack] "i"(&_stack_section_end), - [sstack] "i"((uint32_t)&_stack_section_start) - : // no clobber - ); + call_with_new_stack((uint32_t)error_handler, 0, + system_emergency_rescue_phase_2); } #endif // KERNEL_MODE diff --git a/core/site_scons/models/stm32f4_common.py b/core/site_scons/models/stm32f4_common.py index 4843bacb35..70eb42f947 100644 --- a/core/site_scons/models/stm32f4_common.py +++ b/core/site_scons/models/stm32f4_common.py @@ -71,6 +71,7 @@ def stm32f4_common_files(env, defines, sources, paths): "embed/sys/mpu/stm32f4/mpu.c", "embed/sys/pvd/stm32/pvd.c", "embed/sys/startup/stm32/bootutils.c", + "embed/sys/startup/stm32/sysutils.c", "embed/sys/startup/stm32f4/reset_flags.c", "embed/sys/startup/stm32f4/startup_init.c", "embed/sys/startup/stm32f4/vectortable.S", diff --git a/core/site_scons/models/stm32u5_common.py b/core/site_scons/models/stm32u5_common.py index f3ff9e6ac3..1c887a5829 100644 --- a/core/site_scons/models/stm32u5_common.py +++ b/core/site_scons/models/stm32u5_common.py @@ -89,6 +89,7 @@ def stm32u5_common_files(env, defines, sources, paths): "embed/sys/mpu/stm32u5/mpu.c", "embed/sys/pvd/stm32/pvd.c", "embed/sys/startup/stm32/bootutils.c", + "embed/sys/startup/stm32/sysutils.c", "embed/sys/startup/stm32u5/reset_flags.c", "embed/sys/startup/stm32u5/startup_init.c", "embed/sys/startup/stm32u5/vectortable.S",