diff --git a/components/esp_tee/include/esp_tee.h b/components/esp_tee/include/esp_tee.h
index c22d887b49..a529928897 100644
--- a/components/esp_tee/include/esp_tee.h
+++ b/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");
diff --git a/components/esp_tee/src/esp_tee_config.c b/components/esp_tee/src/esp_tee_config.c
index eec0210f9e..62f5051b31 100644
--- a/components/esp_tee/src/esp_tee_config.c
+++ b/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,
diff --git a/components/esp_tee/subproject/main/CMakeLists.txt b/components/esp_tee/subproject/main/CMakeLists.txt
index 4be567ca54..9dfeb7e527 100644
--- a/components/esp_tee/subproject/main/CMakeLists.txt
+++ b/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
diff --git a/components/esp_tee/subproject/main/arch/riscv/esp_tee_secure_entry.S b/components/esp_tee/subproject/main/arch/riscv/esp_tee_secure_entry.S
deleted file mode 100644
index 333952a247..0000000000
--- a/components/esp_tee/subproject/main/arch/riscv/esp_tee_secure_entry.S
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
- *
- * SPDX-License-Identifier: Apache-2.0
- */
-#include "soc/tee_reg.h"
-#include "soc/plic_reg.h"
-
-    .global   esp_tee_service_dispatcher
-
-/* Entry point to the secure world (i.e. M-mode) - responsible for
- * setting up the execution environment for the secure world */
-    .section .text
-    .align  4
-    .global _sec_world_entry
-    .type _sec_world_entry, @function
-_sec_world_entry:
-    /* Disable the U-mode delegation of all interrupts */
-    csrwi   mideleg, 0
-
-    /* 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, 0xffffbfff
-    csrw    mideleg, t0
-
-    /* Fire an M-ecall */
-    mv      a1, zero
-    ecall
-    fence
-
-    ret
diff --git a/components/esp_tee/subproject/main/arch/riscv/esp_tee_vectors.S b/components/esp_tee/subproject/main/arch/riscv/esp_tee_vectors.S
index 92a3a80938..2028bec395 100644
--- a/components/esp_tee/subproject/main/arch/riscv/esp_tee_vectors.S
+++ b/components/esp_tee/subproject/main/arch/riscv/esp_tee_vectors.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
 
diff --git a/components/esp_tee/subproject/main/core/esp_secure_dispatcher.c b/components/esp_tee/subproject/main/core/esp_secure_dispatcher.c
index 323c715164..2b77e1fa7f 100644
--- a/components/esp_tee/subproject/main/core/esp_secure_dispatcher.c
+++ b/components/esp_tee/subproject/main/core/esp_secure_dispatcher.c
@@ -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,7 +47,7 @@ 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--;
@@ -58,13 +55,11 @@ int esp_tee_service_dispatcher(int argc, va_list ap)
     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
 
+        "li t2, 8 \n"              // t2 = 8 (max register args)
+        "ble t0, t2, load_regs \n" // If argc <= 8 (a0-a7), skip stack routine
+
+        // Store extra args (argc > 8) on stack
+        "mv t3, sp \n"
+        "addi t1, t1, 32 \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"
+        "bge t0, t2, stack_loop \n"
+
+        // Load the first 8 arguments into a0-a7
+        "load_regs: \n"
         "lw a0, 0(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
-
         "lw a1, 4(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \n"
-
         "lw a2, 8(%3) \n"
-        "addi t0, t0, -1 \n"
-        "beqz t0, service_call \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
diff --git a/components/esp_tee/subproject/main/core/esp_tee_init.c b/components/esp_tee/subproject/main/core/esp_tee_init.c
index 6976aa561b..995f282769 100644
--- a/components/esp_tee/subproject/main/core/esp_tee_init.c
+++ b/components/esp_tee/subproject/main/core/esp_tee_init.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;
 }
 
diff --git a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/include/esp_tee_test.h b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/include/esp_tee_test.h
index 0830bfb8be..705383e5fc 100644
--- a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/include/esp_tee_test.h
+++ b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/include/esp_tee_test.h
@@ -1,5 +1,5 @@
 /*
- * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -10,16 +10,17 @@ extern "C" {
 #endif
 
 #include <stdint.h>
+#include "esp_attr.h"
 
 #define TEE_TEST_INT_COUNT 3
 
-uint32_t __attribute__((__noinline__)) esp_tee_service_add(uint32_t a, uint32_t b);
+uint32_t NOINLINE_ATTR esp_tee_service_add(uint32_t a, uint32_t b);
 
-uint32_t __attribute__((__noinline__)) esp_tee_service_sub(uint32_t a, uint32_t b);
+uint32_t NOINLINE_ATTR esp_tee_service_sub(uint32_t a, uint32_t b);
 
-uint32_t __attribute__((__noinline__)) esp_tee_service_mul(uint32_t a, uint32_t b);
+uint32_t NOINLINE_ATTR esp_tee_service_mul(uint32_t a, uint32_t b);
 
-uint32_t __attribute__((__noinline__)) esp_tee_service_div(uint32_t a, uint32_t b);
+uint32_t NOINLINE_ATTR esp_tee_service_div(uint32_t a, uint32_t b);
 
 int esp_tee_secure_int_test(void);
 
@@ -33,7 +34,7 @@ int esp_tee_test_illegal_instr(void);
 
 int esp_tee_test_instr_fetch_prohibited(uint32_t type);
 
-void dummy_secure_service(void);
+void NOINLINE_ATTR dummy_secure_service(int a, int b, int c, int d, int e, int f, int g, int h, int *i);
 
 uint32_t add_in_loop(uint32_t a, uint32_t b, uint32_t iter);
 
diff --git a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/sec_srv_tbl_test.yml b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/sec_srv_tbl_test.yml
index 76ba15c605..5945326e7a 100644
--- a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/sec_srv_tbl_test.yml
+++ b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/sec_srv_tbl_test.yml
@@ -64,7 +64,7 @@ secure_services:
       - id: 215
         type: custom
         function: dummy_secure_service
-        args: 0
+        args: 9
       - id: 216
         type: custom
         function: add_in_loop
diff --git a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv.c b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv.c
index 24faeb5e41..7562516a77 100644
--- a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv.c
+++ b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv.c
@@ -7,8 +7,10 @@
 #include "esp_tee.h"
 #include "esp_err.h"
 #include "esp_rom_sys.h"
+#include "esp_attr.h"
 
-void _ss_dummy_secure_service(void)
+void NOINLINE_ATTR _ss_dummy_secure_service(int a, int b, int c, int d, int e, int f, int g, int h, int *i)
 {
     esp_rom_printf("Dummy secure service\n");
+    *i = a + b + c + d + e + f + g + h;
 }
diff --git a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv_wrapper.c b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv_wrapper.c
index ae40a94193..d31b968ce7 100644
--- a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv_wrapper.c
+++ b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_dummy_srv_wrapper.c
@@ -8,7 +8,7 @@
 #include "esp_tee.h"
 #include "esp_err.h"
 
-void dummy_secure_service(void)
+void dummy_secure_service(int a, int b, int c, int d, int e, int f, int g, int h, int *i)
 {
-    esp_tee_service_call(1, SS_DUMMY_SECURE_SERVICE);
+    esp_tee_service_call(10, SS_DUMMY_SECURE_SERVICE, a, b, c, d, e, f, g, h, i);
 }
diff --git a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_sec_srv.c b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_sec_srv.c
index 8cf63bf4e1..4fc14500cf 100644
--- a/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_sec_srv.c
+++ b/components/esp_tee/test_apps/tee_test_fw/components/test_sec_srv/src/test_sec_srv.c
@@ -1,5 +1,5 @@
 /*
- * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -7,30 +7,30 @@
 #include "esp_log.h"
 #include "esp_tee.h"
 #include "esp_tee_test.h"
-
+#include "esp_attr.h"
 static const char *TAG = "test_sec_srv";
 
 /* Sample Trusted App */
 
-uint32_t __attribute__((__noinline__)) _ss_esp_tee_service_add(uint32_t a, uint32_t b)
+uint32_t NOINLINE_ATTR _ss_esp_tee_service_add(uint32_t a, uint32_t b)
 {
     ESP_LOGD(TAG, "SS: %s", __func__);
     return (a + b);
 }
 
-uint32_t __attribute__((__noinline__)) _ss_esp_tee_service_sub(uint32_t a, uint32_t b)
+uint32_t NOINLINE_ATTR _ss_esp_tee_service_sub(uint32_t a, uint32_t b)
 {
     ESP_LOGD(TAG, "SS: %s", __func__);
     return (a - b);
 }
 
-uint32_t __attribute__((__noinline__)) _ss_esp_tee_service_mul(uint32_t a, uint32_t b)
+uint32_t NOINLINE_ATTR _ss_esp_tee_service_mul(uint32_t a, uint32_t b)
 {
     ESP_LOGD(TAG, "SS: %s", __func__);
     return (a * b);
 }
 
-uint32_t __attribute__((__noinline__)) _ss_esp_tee_service_div(uint32_t a, uint32_t b)
+uint32_t NOINLINE_ATTR _ss_esp_tee_service_div(uint32_t a, uint32_t b)
 {
     ESP_LOGD(TAG, "SS: %s", __func__);
     return (a / b);
diff --git a/components/esp_tee/test_apps/tee_test_fw/main/test_esp_tee_ctx_switch.c b/components/esp_tee/test_apps/tee_test_fw/main/test_esp_tee_ctx_switch.c
index 298d3ade3e..474e2c3430 100644
--- a/components/esp_tee/test_apps/tee_test_fw/main/test_esp_tee_ctx_switch.c
+++ b/components/esp_tee/test_apps/tee_test_fw/main/test_esp_tee_ctx_switch.c
@@ -1,5 +1,5 @@
 /*
- * SPDX-FileCopyrightText: 2024 Espressif Systems (Shanghai) CO LTD
+ * SPDX-FileCopyrightText: 2024-2025 Espressif Systems (Shanghai) CO LTD
  *
  * SPDX-License-Identifier: Apache-2.0
  */
@@ -68,7 +68,9 @@ TEST_CASE("Test multiple calls to sample app (basic services)", "[basic]")
 
 TEST_CASE("Custom secure service call", "[basic]")
 {
-    dummy_secure_service();
+    int res = -1;
+    dummy_secure_service(1, 2, 3, 4, 5, 6, 7, 8, &res);
+    TEST_ASSERT_EQUAL_UINT32(36, res);
 }
 
 void test_task(void *pvParameters)