refactor(esp_tee): Simplify service call ASM routine

- Remove `mret` for jumping to the service call dispatcher; instead, enable
  interrupts and execute directly
- Fix potential corruption of the `t3` register when returning from a service
  call
- Simplify the secure service dispatcher function

Author: laukik-hase

components/esp_tee/include/esp_tee.h

@@ -43,9 +43,8 @@ typedef struct {
     uint32_t magic_word;
     uint32_t api_major_version;
     uint32_t api_minor_version;
-    uint32_t reserved[2];
+    uint32_t reserved[3];
     /* TEE-related fields */
-    void *s_entry_addr;
     void *s_int_handler;
     /* REE-related fields */
     void *ns_entry_addr;
@@ -85,14 +84,12 @@ uint32_t esp_tee_service_call_with_noniram_intr_disabled(int argc, ...);
 
 #if !(__DOXYGEN__)
 /* Offsets of some values in esp_tee_config_t that are used by assembly code */
-#define ESP_TEE_CFG_OFFS_S_ENTRY_ADDR      0x14
 #define ESP_TEE_CFG_OFFS_S_INTR_HANDLER    0x18
 #define ESP_TEE_CFG_OFFS_NS_ENTRY_ADDR     0x1C
 #define ESP_TEE_CFG_OFFS_NS_INTR_HANDLER   0x20
 
 #if !defined(__ASSEMBLER__)
 /* Check the offsets are correct using the C compiler */
-ESP_STATIC_ASSERT(offsetof(esp_tee_config_t, s_entry_addr) == ESP_TEE_CFG_OFFS_S_ENTRY_ADDR, "offset macro is wrong");
 ESP_STATIC_ASSERT(offsetof(esp_tee_config_t, s_int_handler) == ESP_TEE_CFG_OFFS_S_INTR_HANDLER, "offset macro is wrong");
 ESP_STATIC_ASSERT(offsetof(esp_tee_config_t, ns_entry_addr) == ESP_TEE_CFG_OFFS_NS_ENTRY_ADDR, "offset macro is wrong");
 ESP_STATIC_ASSERT(offsetof(esp_tee_config_t, ns_int_handler) == ESP_TEE_CFG_OFFS_NS_INTR_HANDLER, "offset macro is wrong");

components/esp_tee/src/esp_tee_config.c

@@ -1,5 +1,5 @@
 /*
- * SPDX-FileCopyrightText: 2021-2024 Espressif Systems (Shanghai) CO LTD
+ * SPDX-FileCopyrightText: 2021-2025 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -25,9 +25,9 @@ esp_tee_config_t esp_tee_app_config __attribute__((section(".esp_tee_app_cfg")))
     .api_major_version = ESP_TEE_API_MAJOR_VER,
     .api_minor_version = ESP_TEE_API_MINOR_VER,
 
-    /* .s_entry_addr and .s_intr_handler are NULL in the
-       app binary, but will be written by the TEE before it loads the binary
-    */
+    /* s_intr_handler is NULL in the REE image, but will be written by
+     * the TEE before it loads the binary
+     */
 
     .ns_int_handler = &_tee_interrupt_handler,
     .ns_entry_addr = &_u2m_switch,

components/esp_tee/subproject/main/CMakeLists.txt

@@ -20,8 +20,7 @@ set(srcs "core/esp_tee_init.c"
 
 # Arch specific implementation for TEE
 list(APPEND srcs "arch/${arch}/esp_tee_vectors.S"
-                 "arch/${arch}/esp_tee_vector_table.S"
-                 "arch/${arch}/esp_tee_secure_entry.S")
+                 "arch/${arch}/esp_tee_vector_table.S")
 
 # SoC specific implementation for TEE
 list(APPEND srcs "soc/${target}/esp_tee_secure_sys_cfg.c"
@@ -78,7 +77,9 @@ list(APPEND srcs "common/esp_app_desc_tee.c")
 idf_component_register(SRCS ${srcs}
                        INCLUDE_DIRS ${include})
 
-set_source_files_properties("core/esp_secure_services.c" PROPERTIES COMPILE_FLAGS -Wno-deprecated)
+# TODO: Currently only -Og optimization level works correctly at runtime
+set_source_files_properties("core/esp_secure_dispatcher.c" PROPERTIES COMPILE_FLAGS "-Og")
+
 include(${CMAKE_CURRENT_LIST_DIR}/ld/esp_tee_ld.cmake)
 
 # esp_app_desc_t configuration structure for TEE: Linking symbol and trimming project version and name

components/esp_tee/subproject/main/arch/riscv/esp_tee_secure_entry.S

@@ -12,6 +12,7 @@
 
 #include "riscv/encoding.h"
 #include "riscv/rvruntime-frames.h"
+#include "esp_private/vectors_const.h"
 
 #include "esp_tee.h"
 #include "sdkconfig.h"
@@ -25,9 +26,12 @@
     .equ ECALL_U_MODE, 0x8
     .equ ECALL_M_MODE, 0xb
     .equ TEE_APM_INTR_MASK_0, 0x00300000
-    .equ TEE_APM_INTR_MASK_1, 0x000000F8
+    .equ TEE_APM_INTR_MASK_1, 0x000000f8
+    .equ TEE_INTR_DELEG_MASK, 0xffffbfff
 
     .global esp_tee_global_interrupt_handler
+    .global   esp_tee_service_dispatcher
+
 
     .section .data
     .align 4
@@ -179,6 +183,8 @@ _panic_handler:
 
     /* Read mcause */
     csrr    t0, mcause
+    li      t1, VECTORS_MCAUSE_INTBIT_MASK | VECTORS_MCAUSE_REASON_MASK
+    and     t0, t0, t1
 
     /* Check whether the exception is an M-mode ecall */
     li      t1, ECALL_M_MODE
@@ -291,28 +297,34 @@ _user_ecall:
     lw      t0, 0(sp)
     addi    sp, sp, 16
 
-    /* This point is reached when a service call is issued from the REE */
+    /* This point is reached when a secure service call is issued from the REE */
     /* Save register context and mepc */
     save_general_regs RV_STK_FRMSZ
     save_mepc
 
-    /* Saving the U-mode (i.e. REE) stack pointer */
+    /* Save the U-mode (i.e. REE) stack pointer */
     la      t0, _ns_sp
     sw      sp, 0(t0)
 
-    /* Switching to the M-mode (i.e. TEE) stack */
+    /* Switch to the M-mode (i.e. TEE) stack */
     la      sp, _tee_stack
 
-    /* Load the TEE entry point (see sec_world_entry) in the mepc */
-    la      t2, esp_tee_app_config
-    lw      t2, ESP_TEE_CFG_OFFS_S_ENTRY_ADDR(t2)
-    csrw    mepc, t2
+    /* Disable the U-mode delegation of all interrupts */
+    csrwi   mideleg, 0
 
-    /* Set the privilege mode to transition to after mret to M-mode  */
-    li      t3, MSTATUS_MPP
-    csrs    mstatus, t3
+    /* Enable interrupts */
+    csrsi   mstatus, MSTATUS_MIE
 
-    mret
+    /* Jump to the secure service dispatcher */
+    jal     esp_tee_service_dispatcher
+
+    /* Enable the U-mode delegation of all interrupts (except the TEE secure interrupt) */
+    li      t0, TEE_INTR_DELEG_MASK
+    csrs    mideleg, t0
+
+    /* Fire an M-ecall */
+    mv      a1, zero
+    ecall
 
     /* This point is reached after servicing a U-mode interrupt occurred
      * while executing a secure service */
@@ -333,7 +345,7 @@ _rtn_from_ns_int:
 
     /* Restore register context and resume the secure service */
     restore_mepc
-    restore_general_regs
+    restore_general_regs RV_STK_FRMSZ
 
     mret
 
@@ -347,7 +359,7 @@ _rtn_from_ns_int:
 _tee_ns_intr_handler:
     /* Start by saving the general purpose registers and the PC value before
      * the interrupt happened. */
-    save_general_regs
+    save_general_regs RV_STK_FRMSZ
     save_mepc
 
     /* Though it is not necessary we save GP and SP here.
@@ -357,7 +369,7 @@ _tee_ns_intr_handler:
     /* As gp register is not saved by the macro, save it here */
     sw      gp, RV_STK_GP(sp)
     /* Same goes for the SP value before trapping */
-    addi    t0, sp, CONTEXT_SIZE /* restore sp with the value when interrupt happened */
+    addi    t0, sp, RV_STK_FRMSZ /* restore sp with the value when interrupt happened */
     /* Save SP */
     sw      t0, RV_STK_SP(sp)
 
@@ -395,8 +407,8 @@ _tee_ns_intr_handler:
     csrw    mscratch, t0
 
     /* Enable the U-mode interrupt delegation (except for the TEE secure interrupt) */
-    li      t0, 0xffffbfff
-    csrw    mideleg, t0
+    li      t0, TEE_INTR_DELEG_MASK
+    csrs    mideleg, t0
 
     /* Place magic bytes in all the general registers */
     store_magic_general_regs
@@ -413,7 +425,7 @@ _tee_ns_intr_handler:
 _tee_s_intr_handler:
     /* Start by saving the general purpose registers and the PC value before
      * the interrupt happened. */
-    save_general_regs
+    save_general_regs RV_STK_FRMSZ
     save_mepc
 
     /* Though it is not necessary we save GP and SP here.
@@ -423,7 +435,7 @@ _tee_s_intr_handler:
     /* As gp register is not saved by the macro, save it here */
     sw      gp, RV_STK_GP(sp)
     /* Same goes for the SP value before trapping */
-    addi    t0, sp, CONTEXT_SIZE /* restore sp with the value when interrupt happened */
+    addi    t0, sp, RV_STK_FRMSZ /* restore sp with the value when interrupt happened */
     /* Save SP */
     sw      t0, RV_STK_SP(sp)
 
@@ -457,7 +469,7 @@ _save_reg_ctx:
 _continue:
     /* Before doing anything preserve the stack pointer */
     mv      s11, sp
-    /* Switching to the TEE interrupt stack */
+    /* Switch to the TEE interrupt stack */
     la      sp, _tee_intr_stack
     /* If this is a non-nested interrupt, SP now points to the interrupt stack */
 
@@ -527,7 +539,7 @@ _intr_hdlr_exec:
     mv      sp, s11
 
     restore_mepc
-    restore_general_regs
+    restore_general_regs RV_STK_FRMSZ
     /* exit, this will also re-enable the interrupts */
     mret
 

components/esp_tee/subproject/main/arch/riscv/esp_tee_vectors.S

@@ -3,7 +3,7 @@
  *
  * SPDX-License-Identifier: Apache-2.0
  */
-
+#include <string.h>
 #include <stdarg.h>
 #include "esp_log.h"
 #include "esp_tee.h"
@@ -34,13 +34,10 @@ static const secure_service_entry_t *find_service_by_id(uint32_t id)
 }
 
 /**
- * @brief Entry point to the TEE binary during secure service call. It decipher the call and dispatch it
- *        to corresponding Secure Service API in secure world.
- * TODO: Fix the assembly routine here for compatibility with all levels of compiler optimizations
+ * @brief Handles incoming secure service requests to the TEE.
+ *        Validates and routes each request to the appropriate
+ *        secure service implementation.
  */
-#pragma GCC push_options
-#pragma GCC optimize ("Og")
-
 int esp_tee_service_dispatcher(int argc, va_list ap)
 {
     if (argc > ESP_TEE_MAX_INPUT_ARG) {
@@ -50,21 +47,19 @@ int esp_tee_service_dispatcher(int argc, va_list ap)
     }
 
     int ret = -1;
-    uint32_t argv[ESP_TEE_MAX_INPUT_ARG], *argp;
+    uint32_t argv[ESP_TEE_MAX_INPUT_ARG] = {0};
 
     uint32_t sid = va_arg(ap, uint32_t);
     argc--;
 
     const secure_service_entry_t *service = find_service_by_id(sid);
     if (service == NULL) {
         ESP_LOGE(TAG, "Invalid service ID!");
-        va_end(ap);
         return ret;
     }
 
     if (argc != service->nargs) {
         ESP_LOGE(TAG, "Invalid number of arguments for service %d!", sid);
-        va_end(ap);
         return ret;
     }
 
@@ -73,65 +68,47 @@ int esp_tee_service_dispatcher(int argc, va_list ap)
     for (int i = 0; i < argc; i++) {
         argv[i] = va_arg(ap, uint32_t);
     }
-    argp = &argv[0];
-    va_end(ap);
+    uint32_t *argp = &argv[0];
 
     asm volatile(
-        "mv t0, %1 \n"
-        "beqz t0, service_call \n"
+        "mv t0, %1 \n"            // t0 = argc
+        "mv t1, %3 \n"            // t1 = argp
 
-        "lw a0, 0(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
+        "li t2, 8 \n"              // t2 = 8 (max register args)
+        "ble t0, t2, load_regs \n" // If argc <= 8 (a0-a7), skip stack routine
 
-        "lw a1, 4(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
+        // Store extra args (argc > 8) on stack
+        "mv t3, sp \n"
+        "addi t1, t1, 32 \n"
 
-        "lw a2, 8(%3) \n"
+        "stack_loop: \n"
+        "lw t4, 0(t1) \n"
+        "sw t4, 0(t3) \n"
+        "addi t1, t1, 4 \n"
+        "addi t3, t3, 4 \n"
         "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
+        "bge t0, t2, stack_loop \n"
 
+        // Load the first 8 arguments into a0-a7
+        "load_regs: \n"
+        "lw a0, 0(%3) \n"
+        "lw a1, 4(%3) \n"
+        "lw a2, 8(%3) \n"
         "lw a3, 12(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
-
         "lw a4, 16(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
-
         "lw a5, 20(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
-
         "lw a6, 24(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
-
         "lw a7, 28(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
+        "fence \n"
 
-        "addi %3, %3, 32 \n"
-        "mv t2, sp \n"
-        "loop: \n"
-        "lw t1, 0(%3) \n"
-        "sw t1, 0(t2) \n"
-        "addi t0, t0, -1 \n"
-        "addi t2, t2, 4 \n"
-        "addi %3, %3, 4 \n"
-        "bnez t0, loop \n"
-
-        "service_call: \n"
-        "mv t1, %2 \n"
-        "jalr 0(t1) \n"
-        "mv %0, a0 \n"
+        "mv t1, %2 \n"            // Load function pointer
+        "jalr 0(t1) \n"           // Call function
+        "mv %0, a0 \n"            // Store return value
         : "=r"(ret)
         : "r"(argc), "r"(fp_secure_service), "r"(argp)
-        : "a0", "a1", "a2", "a3", "a4", "a5", "a6", "a7", "t0", "t1", "t2"
+        : "a0", "a1", "a2", "a3", "a4", "a5", "a6", "a7",
+        "t0", "t1", "t2", "t3", "t4"
     );
 
     return ret;
 }
-
-#pragma GCC pop_options

components/esp_tee/subproject/main/core/esp_secure_dispatcher.c

@@ -59,7 +59,6 @@ static void tee_init_app_config(void)
     esp_tee_app_config.api_minor_version = ESP_TEE_API_MINOR_VER;
 
     /* Set the TEE-related fields (from the TEE binary) that the REE will use to interface with TEE */
-    esp_tee_app_config.s_entry_addr = &_sec_world_entry;
     esp_tee_app_config.s_int_handler = &_tee_s_intr_handler;
 }