diff --git a/core/embed/firmware/main.c b/core/embed/firmware/main.c index f81ca9dbb8..68be5de804 100644 --- a/core/embed/firmware/main.c +++ b/core/embed/firmware/main.c @@ -40,6 +40,7 @@ #include "mpu.h" #include "rng.h" #include "sdcard.h" +#include "supervise.h" #include "touch.h" int main(void) { @@ -113,6 +114,36 @@ void __attribute__((noreturn)) nlr_jump_fail(void *val) { void PendSV_Handler(void) { pendsv_isr_handler(); } +void SVC_C_Handler(uint32_t *stack) { + uint8_t svc_number = ((uint8_t *)stack[6])[-2]; + switch (svc_number) { + case SVC_ENABLE_IRQ: + HAL_NVIC_EnableIRQ(stack[0]); + break; + case SVC_DISABLE_IRQ: + HAL_NVIC_DisableIRQ(stack[0]); + break; + case SVC_SET_PRIORITY: + NVIC_SetPriority(stack[0], stack[1]); + break; + default: + stack[0] = 0xffffffff; + break; + } +} + +__attribute__((naked)) void SVC_Handler(void) { + __asm volatile( + " tst lr, #4 \n" // Test Bit 3 to see which stack pointer we should + // use. + " ite eq \n" // Tell the assembler that the nest 2 instructions + // are if-then-else + " mrseq r0, msp \n" // Make R0 point to main stack pointer + " mrsne r0, psp \n" // Make R0 point to process stack pointer + " b SVC_C_Handler \n" // Off to C land + ); +} + // MicroPython builtin stubs mp_import_stat_t mp_import_stat(const char *path) { diff --git a/core/embed/firmware/supervise.h b/core/embed/firmware/supervise.h new file mode 100644 index 0000000000..d62e89aa8d --- /dev/null +++ b/core/embed/firmware/supervise.h @@ -0,0 +1,22 @@ +// supervisor call functions + +#define SVC_ENABLE_IRQ 0 +#define SVC_DISABLE_IRQ 1 +#define SVC_SET_PRIORITY 2 + +inline void svc_enableIRQ(uint32_t IRQn) { + register uint32_t r0 __asm__("r0") = IRQn; + __asm__ __volatile__("svc %0" ::"i"(SVC_ENABLE_IRQ), "r"(r0) : "memory"); +} + +inline void svc_disableIRQ(uint32_t IRQn) { + register uint32_t r0 __asm__("r0") = IRQn; + __asm__ __volatile__("svc %0" ::"i"(SVC_DISABLE_IRQ), "r"(r0) : "memory"); +} + +inline void svc_setpriority(uint32_t IRQn, uint32_t priority) { + register uint32_t r0 __asm__("r0") = IRQn; + register uint32_t r1 __asm__("r1") = priority; + __asm__ __volatile__("svc %0" ::"i"(SVC_SET_PRIORITY), "r"(r0), "r"(r1) + : "memory"); +} diff --git a/core/embed/trezorhal/dma.c b/core/embed/trezorhal/dma.c index 897773ffd2..6126560200 100644 --- a/core/embed/trezorhal/dma.c +++ b/core/embed/trezorhal/dma.c @@ -33,6 +33,7 @@ #include "dma.h" #include "irq.h" #include "systick.h" +#include "supervise.h" #define DMA_IDLE_ENABLED() (dma_idle.enabled != 0) #define DMA_SYSTICK_LOG2 (3) @@ -233,7 +234,7 @@ void dma_init(DMA_HandleTypeDef *dma, const dma_descr_t *dma_descr, uint32_t dir // (dma->State is set to HAL_DMA_STATE_RESET by memset above) HAL_DMA_DeInit(dma); HAL_DMA_Init(dma); - NVIC_SetPriority(IRQn_NONNEG(dma_irqn[dma_id]), IRQ_PRI_DMA); + svc_setpriority(IRQn_NONNEG(dma_irqn[dma_id]), IRQ_PRI_DMA); } else { // only necessary initialization dma->State = HAL_DMA_STATE_READY; @@ -242,13 +243,13 @@ void dma_init(DMA_HandleTypeDef *dma, const dma_descr_t *dma_descr, uint32_t dir DMA_CalcBaseAndBitshift(dma); } - HAL_NVIC_EnableIRQ(dma_irqn[dma_id]); + svc_enableIRQ(dma_irqn[dma_id]); } } void dma_deinit(const dma_descr_t *dma_descr) { if (dma_descr != NULL) { - HAL_NVIC_DisableIRQ(dma_irqn[dma_descr->id]); + svc_disableIRQ(dma_irqn[dma_descr->id]); dma_handle[dma_descr->id] = NULL; dma_disable_clock(dma_descr->id); diff --git a/core/embed/trezorhal/sdcard.c b/core/embed/trezorhal/sdcard.c index 15170774d5..6dbe4c3bdd 100644 --- a/core/embed/trezorhal/sdcard.c +++ b/core/embed/trezorhal/sdcard.c @@ -52,6 +52,7 @@ #include "irq.h" #include "sdcard-set_clr_card_detect.h" #include "sdcard.h" +#include "supervise.h" #define SDMMC_CLK_ENABLE() __HAL_RCC_SDMMC1_CLK_ENABLE() #define SDMMC_CLK_DISABLE() __HAL_RCC_SDMMC1_CLK_DISABLE() @@ -122,8 +123,8 @@ void HAL_SD_MspInit(SD_HandleTypeDef *hsd) { SDMMC_CLK_ENABLE(); // NVIC configuration for SDIO interrupts - NVIC_SetPriority(SDMMC_IRQn, IRQ_PRI_SDIO); - HAL_NVIC_EnableIRQ(SDMMC_IRQn); + svc_setpriority(SDMMC_IRQn, IRQ_PRI_SDIO); + svc_enableIRQ(SDMMC_IRQn); } // GPIO have already been initialised by sdcard_init @@ -131,7 +132,7 @@ void HAL_SD_MspInit(SD_HandleTypeDef *hsd) { void HAL_SD_MspDeInit(SD_HandleTypeDef *hsd) { if (hsd->Instance == sd_handle.Instance) { - HAL_NVIC_DisableIRQ(SDMMC_IRQn); + svc_disableIRQ(SDMMC_IRQn); SDMMC_CLK_DISABLE(); } }