mirror of
https://github.com/trezor/trezor-firmware.git
synced 2025-02-21 12:02:19 +00:00
refactor(core): rewrite rescue code in C
[no changelog]
This commit is contained in:
parent
95afa34f27
commit
2697c06642
core
embed
projects/firmware
sys
linker
startup
task/stm32
site_scons/models
@ -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;
|
||||
|
@ -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,
|
||||
|
@ -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"
|
||||
|
@ -20,6 +20,7 @@
|
||||
#ifndef TREZORHAL_BOOTARGS_H
|
||||
#define TREZORHAL_BOOTARGS_H
|
||||
|
||||
#include <sys/systask.h>
|
||||
#include <trezor_types.h>
|
||||
|
||||
// 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
|
||||
|
46
core/embed/sys/startup/inc/sys/sysutils.h
Normal file
46
core/embed/sys/startup/inc/sys/sysutils.h
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#ifdef KERNEL_MODE
|
||||
|
||||
#include <trezor_types.h>
|
||||
|
||||
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
|
@ -26,6 +26,7 @@
|
||||
#include <sys/bootutils.h>
|
||||
#include <sys/irq.h>
|
||||
#include <sys/mpu.h>
|
||||
#include <sys/sysutils.h>
|
||||
#include <util/image.h>
|
||||
|
||||
#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) {
|
||||
|
215
core/embed/sys/startup/stm32/sysutils.c
Normal file
215
core/embed/sys/startup/stm32/sysutils.c
Normal file
@ -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 <http://www.gnu.org/licenses/>.
|
||||
*/
|
||||
|
||||
#include <trezor_bsp.h>
|
||||
|
||||
#include <sys/linker_utils.h>
|
||||
#include <sys/sysutils.h>
|
||||
|
||||
#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
|
@ -26,6 +26,8 @@
|
||||
#include <sys/linker_utils.h>
|
||||
#include <sys/system.h>
|
||||
#include <sys/systick.h>
|
||||
#include <sys/sysutils.h>
|
||||
|
||||
#include "startup_init.h"
|
||||
|
||||
#ifdef KERNEL_MODE
|
||||
|
@ -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
|
||||
|
@ -22,6 +22,7 @@
|
||||
|
||||
#include <sys/bootutils.h>
|
||||
#include <sys/irq.h>
|
||||
#include <sys/linker_utils.h>
|
||||
#include <sys/mpu.h>
|
||||
#include <sys/syscall.h>
|
||||
#include <sys/systask.h>
|
||||
@ -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
|
||||
|
@ -20,12 +20,15 @@
|
||||
#include <trezor_bsp.h>
|
||||
#include <trezor_rtl.h>
|
||||
|
||||
#include <sys/bootargs.h>
|
||||
#include <sys/bootutils.h>
|
||||
#include <sys/linker_utils.h>
|
||||
#include <sys/mpu.h>
|
||||
#include <sys/systask.h>
|
||||
#include <sys/system.h>
|
||||
#include <sys/systick.h>
|
||||
#include <sys/systimer.h>
|
||||
#include <sys/sysutils.h>
|
||||
|
||||
#ifdef USE_SDRAM
|
||||
#include <sys/sdram.h>
|
||||
@ -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
|
||||
|
@ -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",
|
||||
|
@ -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",
|
||||
|
Loading…
Reference in New Issue
Block a user