diff --git a/secure_fw/spm/core/arch/tfm_arch_v6m_v7m.c b/secure_fw/spm/core/arch/tfm_arch_v6m_v7m.c
index b21af9e..85fc224 100644
--- a/secure_fw/spm/core/arch/tfm_arch_v6m_v7m.c
+++ b/secure_fw/spm/core/arch/tfm_arch_v6m_v7m.c
@@ -140,17 +140,19 @@
     "PUSH    {r1, r2}                       \n" /* Orig_exc_return, dummy */
     "BL      spm_svc_handler                \n"
     "MOV     lr, r0                         \n"
-    "POP     {r1, r2}                       \n" /* Orig_exc_return, dummy */
+    "LDR     r1, [sp]                       \n" /* Get orig_exc_return value */
     "MOVS    r2, #8                         \n"
     "ANDS    r0, r2                         \n" /* Mode bit */
     "ANDS    r1, r2                         \n"
-    "POP     {r2, r3}                       \n" /* PSP dummy */
     "SUBS    r0, r1                         \n" /* Compare EXC_RETURN values */
     "BGT     to_flih_func                   \n"
     "BLT     from_flih_func                 \n"
+    "ADD     sp, #16                        \n" /*
+                                                 * "Unstack" unused orig_exc_return, dummy,
+                                                 * PSP, PSPLIM pushed by current handler
+                                                 */
     "BX      lr                             \n"
     "to_flih_func:                          \n"
-    "PUSH    {r2, r3}                       \n" /* PSP dummy */
     "PUSH    {r4-r7}                        \n"
     "MOV     r4, r8                         \n"
     "MOV     r5, r9                         \n"
@@ -172,6 +174,10 @@
     "PUSH    {r4, r5}                       \n" /* Seal stack before EXC_RET */
     "BX      lr                             \n"
     "from_flih_func:                        \n"
+    "ADD     sp, #16                        \n" /*
+                                                 * "Unstack" unused orig_exc_return, dummy,
+                                                 * PSP, PSPLIM pushed by current handler
+                                                 */
     "POP     {r4, r5}                       \n" /* Seal stack */
     "ADD     sp, sp, #8                     \n" /* Dummy data to align SP offset for
                                                  * reserved additional state context,
@@ -183,7 +189,11 @@
     "MOV     r10, r6                        \n"
     "MOV     r11, r7                        \n"
     "POP     {r4-r7}                        \n"
-    "POP     {r1, r2}                       \n" /* PSP dummy */
+    "ADD     sp, #16                        \n" /*
+                                                 * "Unstack" unused orig_exc_return, dummy,
+                                                 * PSP, PSPLIM pushed by the previous
+                                                 * TFM_SVC_PREPARE_DEPRIV_FLIH request
+                                                 */
     "BX      lr                             \n"
     );
 }
diff --git a/secure_fw/spm/core/arch/tfm_arch_v8m_base.c b/secure_fw/spm/core/arch/tfm_arch_v8m_base.c
index b7f9e96..e9127cf 100644
--- a/secure_fw/spm/core/arch/tfm_arch_v8m_base.c
+++ b/secure_fw/spm/core/arch/tfm_arch_v8m_base.c
@@ -173,18 +173,19 @@
     "PUSH    {r1, r2}                       \n" /* Orig_exc_return, dummy */
     "BL      spm_svc_handler                \n"
     "MOV     lr, r0                         \n"
-    "POP     {r1, r2}                       \n" /* Orig_exc_return, dummy */
+    "LDR     r1, [sp]                       \n" /* Get orig_exc_return value */
     "MOVS    r2, #8                         \n"
     "ANDS    r0, r2                         \n" /* Mode bit */
     "ANDS    r1, r2                         \n"
-    "POP     {r2, r3}                       \n" /* PSP PSPLIM */
     "SUBS    r0, r1                         \n" /* Compare EXC_RETURN values */
     "BGT     to_flih_func                   \n"
     "BLT     from_flih_func                 \n"
+    "ADD     sp, #16                        \n" /*
+                                                 * "Unstack" unused orig_exc_return, dummy,
+                                                 * PSP, PSPLIM pushed by current handler
+                                                 */
     "BX      lr                             \n"
     "to_flih_func:                          \n"
-    "PUSH    {r2, r3}                       \n" /* PSP PSPLIM */
-    "PUSH    {r0, r1}                       \n"
     "LDR     r0, ="M2S(EXC_RETURN_DCRS)"    \n" /* Check DCRS */
     "MOV     r1, lr                         \n"
     "ANDS    r0, r1                         \n"
@@ -200,7 +201,6 @@
                                                  * integrity signature
                                                  */
     "v8b_svc_callee_saved:                  \n"
-    "POP     {r0, r1}                       \n"
     "LDR     r4, ="M2S(STACK_SEAL_PATTERN)" \n" /* clear r4-r11 */
     "MOV     r5, r4                         \n"
     "MOV     r6, r4                         \n"
@@ -212,8 +212,11 @@
     "PUSH    {r4, r5}                       \n" /* Seal stack before EXC_RET */
     "BX      lr                             \n"
     "from_flih_func:                        \n"
+    "ADD     sp, #16                        \n" /*
+                                                 * "Unstack" unused orig_exc_return, dummy,
+                                                 * PSP, PSPLIM pushed by current handler
+                                                 */
     "POP     {r4, r5}                       \n" /* Seal stack */
-    "PUSH    {r0, r1}                       \n"
     "LDR     r0, ="M2S(EXC_RETURN_DCRS)"    \n" /* Check DCRS */
     "MOV     r1, lr                         \n"
     "ANDS    r0, r1                         \n"
@@ -229,8 +232,11 @@
     "MOV     r11, r7                        \n"
     "POP     {r4-r7}                        \n"
     "v8b_svc_callee_loaded:                 \n"
-    "POP     {r0, r1}                       \n"
-    "POP     {r1, r2}                       \n" /* PSP PSPLIM */
+    "ADD     sp, #16                        \n" /*
+                                                 * "Unstack" unused orig_exc_return, dummy,
+                                                 * PSP, PSPLIM pushed by the previous
+                                                 * TFM_SVC_PREPARE_DEPRIV_FLIH request
+                                                 */
     "BX      lr                             \n"
     );
 }
diff --git a/secure_fw/spm/core/arch/tfm_arch_v8m_main.c b/secure_fw/spm/core/arch/tfm_arch_v8m_main.c
index 639607d..0bec25f 100644
--- a/secure_fw/spm/core/arch/tfm_arch_v8m_main.c
+++ b/secure_fw/spm/core/arch/tfm_arch_v8m_main.c
@@ -150,16 +150,18 @@
     "PUSH    {r1, r2}                        \n" /* Orig_exc_return, dummy */
     "BL      spm_svc_handler                 \n"
     "MOV     lr, r0                          \n"
-    "POP     {r1, r2}                        \n" /* Orig_exc_return, dummy */
-    "POP     {r2, r3}                        \n" /* PSP PSPLIM */
+    "LDR     r1, [sp]                        \n" /* Get orig_exc_return value */
     "AND     r0, #8                          \n" /* Mode bit */
     "AND     r1, #8                          \n"
     "SUBS    r0, r1                          \n" /* Compare EXC_RETURN values */
     "BGT     to_flih_func                    \n"
     "BLT     from_flih_func                  \n"
+    "ADD     sp, #16                         \n" /*
+                                                  * "Unstack" unused orig_exc_return, dummy,
+                                                  * PSP, PSPLIM pushed by current handler
+                                                  */
     "BX      lr                              \n"
     "to_flih_func:                           \n"
-    "PUSH    {r2, r3}                        \n" /* PSP PSPLIM */
     "ANDS    r3, lr, #"M2S(EXC_RETURN_DCRS)" \n" /* Check DCRS */
     "ITT     ne                              \n" /* Skip saving callee */
     "PUSHNE  {r4-r11}                        \n" /* Save callee */
@@ -178,6 +180,10 @@
     "PUSH    {r4, r5}                        \n" /* Seal stack before EXC_RET */
     "BX      lr                              \n"
     "from_flih_func:                         \n"
+    "ADD     sp, #16                         \n" /*
+                                                  * "Unstack" unused orig_exc_return, dummy,
+                                                  * PSP, PSPLIM pushed by current handler
+                                                  */
     "POP     {r4, r5}                        \n" /* Seal stack */
     "ANDS    r3, lr, #"M2S(EXC_RETURN_DCRS)" \n" /* Check DCRS */
     "ITT     ne                              \n" /* Skip loading callee */
@@ -186,7 +192,11 @@
                                                   * integrity signature
                                                   */
     "POPNE   {r4-r11}                        \n" /* Load callee */
-    "POP     {r1, r2}                        \n" /* PSP PSPLIM */
+    "ADD     sp, #16                         \n" /*
+                                                  * "Unstack" unused orig_exc_return, dummy,
+                                                  * PSP, PSPLIM pushed by the previous
+                                                  * TFM_SVC_PREPARE_DEPRIV_FLIH request
+                                                  */
     "BX      lr                              \n"
     );
 }
diff --git a/secure_fw/spm/core/interrupt.c b/secure_fw/spm/core/interrupt.c
index 88b8f0b..c457ef2 100644
--- a/secure_fw/spm/core/interrupt.c
+++ b/secure_fw/spm/core/interrupt.c
@@ -113,7 +113,7 @@
     /* Set FLIH result to the ISR */
     p_ctx_flih_ret->state_ctx.r0 = (uint32_t)result;
 
-    return EXC_RETURN_HANDLER;
+    return p_ctx_flih_ret->exc_return;
 }
 #endif
 
diff --git a/secure_fw/spm/include/tfm_arch.h b/secure_fw/spm/include/tfm_arch.h
index d19177e..f78e7a3 100644
--- a/secure_fw/spm/include/tfm_arch.h
+++ b/secure_fw/spm/include/tfm_arch.h
@@ -130,11 +130,13 @@
  * It is the same when de-privileged FLIH Function is ready to run.
  */
 struct context_flih_ret_t {
-    uint64_t stack_seal;    /* Two words stack seal */
+    uint64_t stack_seal;                  /* Two words stack seal                              */
     struct tfm_additional_context_t addi_ctx;
-    uint32_t psp;       /* PSP when interrupt exception ocurrs              */
-    uint32_t psplim;    /* PSPLIM when interrupt exception ocurrs when      */
-    struct tfm_state_context_t state_ctx; /* ctx on SVC_PREPARE_DEPRIV_FLIH */
+    uint32_t exc_return;                  /* exception return value on SVC_PREPARE_DEPRIV_FLIH */
+    uint32_t dummy;                       /* dummy value for 8 bytes aligned                   */
+    uint32_t psp;                         /* PSP when interrupt exception ocurrs               */
+    uint32_t psplim;                      /* PSPLIM when interrupt exception ocurrs when       */
+    struct tfm_state_context_t state_ctx; /* ctx on SVC_PREPARE_DEPRIV_FLIH                    */
 };
 
 /* Assign stack and stack limit to the context control instance. */