diff --git a/secure_fw/partitions/ns_agent_tz/ns_agent_tz.c b/secure_fw/partitions/ns_agent_tz/ns_agent_tz.c
index 409641e..8411285 100644
--- a/secure_fw/partitions/ns_agent_tz/ns_agent_tz.c
+++ b/secure_fw/partitions/ns_agent_tz/ns_agent_tz.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -7,6 +7,29 @@
 
 #include "compiler_ext_defs.h"
 #include "tfm_arch.h"
+#include "tfm_hal_platform.h"
+
+#if defined(__ICCARM__)
+#pragma required = ns_agent_tz_init_c
+#endif
+
+/*
+ * Initialization before launching NSPE in C.
+ */
+__used static uint32_t ns_agent_tz_init_c(void)
+{
+#ifndef TFM_MULTI_CORE_TOPOLOGY
+    /* SCB_NS.VTOR points to the Non-secure vector table base address */
+    SCB_NS->VTOR = tfm_hal_get_ns_VTOR();
+
+    /* Setups Main stack pointer of the non-secure code */
+    uint32_t ns_msp = tfm_hal_get_ns_MSP();
+
+    __TZ_set_MSP_NS(ns_msp);
+#endif
+
+    return tfm_hal_get_ns_entry_point();
+}
 
 /*
  * 'r0' implicitly holds the address of non-secure entry,
@@ -16,23 +39,25 @@
 {
     __ASM volatile(
 #ifndef __ICCARM__
-        ".syntax unified         \n"
+        ".syntax unified                \n"
 #endif
-        "mov      r4, r0         \n"
-        "movs     r2, #1         \n" /* Clear Bit[0] for S to NS transition */
-        "bics     r4, r2         \n"
-        "mov      r0, r4         \n"
-        "mov      r1, r4         \n"
-        "mov      r2, r4         \n"
-        "mov      r3, r4         \n"
-        "mov      r5, r4         \n"
-        "mov      r6, r4         \n"
-        "mov      r7, r4         \n"
-        "mov      r8, r4         \n"
-        "mov      r9, r4         \n"
-        "mov      r10, r4        \n"
-        "mov      r11, r4        \n"
-        "mov      r12, r4        \n"
-        "bxns     r0             \n"
+        "bl       ns_agent_tz_init_c    \n"
+        "mov      r4, r0                \n"
+        "movs     r2, #1                \n" /* address[0]:0 for NS execution */
+        "bics     r4, r2                \n"
+        "mov      r0, r4                \n"
+        "mov      r1, r4                \n"
+        "mov      r2, r4                \n"
+        "mov      r3, r4                \n"
+        "mov      r5, r4                \n"
+        "mov      r6, r4                \n"
+        "mov      r7, r4                \n"
+        "mov      r8, r4                \n"
+        "mov      r9, r4                \n"
+        "mov      r10, r4               \n"
+        "mov      r11, r4               \n"
+        "mov      r12, r4               \n"
+        "mov      r14, r4               \n"
+        "bxns     r0                    \n"
     );
 }
diff --git a/secure_fw/spm/CMakeLists.txt b/secure_fw/spm/CMakeLists.txt
index 6321d7f..d467053 100755
--- a/secure_fw/spm/CMakeLists.txt
+++ b/secure_fw/spm/CMakeLists.txt
@@ -54,14 +54,12 @@
         $<$<BOOL:${TFM_PSA_API}>:ffm/backend.c>
         $<$<BOOL:${TFM_PSA_API}>:ffm/interrupt.c>
         $<$<BOOL:${TFM_PSA_API}>:cmsis_psa/tfm_core_svcalls_ipc.c>
-        $<$<AND:$<BOOL:${TFM_PSA_API}>,$<NOT:$<BOOL:${TFM_MULTI_CORE_TOPOLOGY}>>>:cmsis_psa/tfm_nspm_ipc.c>
         $<$<BOOL:${TFM_PSA_API}>:cmsis_psa/tfm_pools.c>
         $<$<BOOL:${TFM_PSA_API}>:cmsis_psa/thread.c>
         $<$<NOT:$<BOOL:${TFM_PSA_API}>>:cmsis_func/main.c>
         $<$<NOT:$<BOOL:${TFM_PSA_API}>>:cmsis_func/arch.c>
         $<$<NOT:$<BOOL:${TFM_PSA_API}>>:cmsis_func/spm_func.c>
         $<$<NOT:$<BOOL:${TFM_PSA_API}>>:cmsis_func/tfm_core_svcalls_func.c>
-        $<$<NOT:$<BOOL:${TFM_PSA_API}>>:cmsis_func/tfm_nspm_func.c>
         $<$<BOOL:${TFM_NS_MANAGE_NSID}>:ns_client_ext/tfm_ns_ctx.c>
         ns_client_ext/tfm_spm_ns_ctx.c
         $<$<NOT:$<BOOL:${TFM_PSA_API}>>:cmsis_func/tfm_secure_api.c>
diff --git a/secure_fw/spm/cmsis_func/main.c b/secure_fw/spm/cmsis_func/main.c
index 1a44ebc..b08a0d4 100644
--- a/secure_fw/spm/cmsis_func/main.c
+++ b/secure_fw/spm/cmsis_func/main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2017-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -14,7 +14,6 @@
 #include "tfm_hal_platform.h"
 #include "tfm_hal_isolation.h"
 #include "tfm_irq_list.h"
-#include "tfm_nspm.h"
 #include "tfm_spm_hal.h"
 #include "tfm_spm_log.h"
 #include "tfm_version.h"
@@ -39,6 +38,25 @@
 REGION_DECLARE(Image$$, ARM_LIB_STACK,  $$ZI$$Limit)[];
 REGION_DECLARE(Image$$, ER_INITIAL_PSP,  $$ZI$$Limit)[];
 
+static void configure_ns_code(void)
+{
+    /* SCB_NS.VTOR points to the Non-secure vector table base address */
+    SCB_NS->VTOR = tfm_spm_hal_get_ns_VTOR();
+
+    /* Setups Main stack pointer of the non-secure code */
+    uint32_t ns_msp = tfm_spm_hal_get_ns_MSP();
+
+    __TZ_set_MSP_NS(ns_msp);
+
+    /* Get the address of non-secure code entry point to jump there */
+    uint32_t entry_ptr = tfm_spm_hal_get_ns_entry_point();
+
+    /* Clears LSB of the function address to indicate the function-call
+     * will perform the switch from secure to non-secure
+     */
+    ns_entry = (nsfptr_t)cmse_nsfptr_create(entry_ptr);
+}
+
 static fih_int tfm_core_init(void)
 {
     size_t i;
diff --git a/secure_fw/spm/cmsis_func/tfm_nspm_func.c b/secure_fw/spm/cmsis_func/tfm_nspm_func.c
deleted file mode 100755
index f282590..0000000
--- a/secure_fw/spm/cmsis_func/tfm_nspm_func.c
+++ /dev/null
@@ -1,30 +0,0 @@
-/*
- * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- *
- */
-
-#include "tfm_spm_hal.h"
-#include "tfm_arch.h"
-#include "arch.h"
-#include "tfm_nspm.h"
-
-void configure_ns_code(void)
-{
-    /* SCB_NS.VTOR points to the Non-secure vector table base address */
-    SCB_NS->VTOR = tfm_spm_hal_get_ns_VTOR();
-
-    /* Setups Main stack pointer of the non-secure code */
-    uint32_t ns_msp = tfm_spm_hal_get_ns_MSP();
-
-    __TZ_set_MSP_NS(ns_msp);
-
-    /* Get the address of non-secure code entry point to jump there */
-    uint32_t entry_ptr = tfm_spm_hal_get_ns_entry_point();
-
-    /* Clears LSB of the function address to indicate the function-call
-     * will perform the switch from secure to non-secure
-     */
-    ns_entry = (nsfptr_t)cmse_nsfptr_create(entry_ptr);
-}
diff --git a/secure_fw/spm/cmsis_psa/main.c b/secure_fw/spm/cmsis_psa/main.c
index c0ed946..1777e44 100644
--- a/secure_fw/spm/cmsis_psa/main.c
+++ b/secure_fw/spm/cmsis_psa/main.c
@@ -12,7 +12,6 @@
 #include "spm_ipc.h"
 #include "tfm_hal_isolation.h"
 #include "tfm_hal_platform.h"
-#include "tfm_nspm.h"
 #include "tfm_spm_hal.h"
 #include "tfm_spm_log.h"
 #include "tfm_version.h"
@@ -112,8 +111,6 @@
 
     tfm_core_validate_boot_data();
 
-    configure_ns_code();
-
     FIH_RET(fih_int_encode(TFM_SUCCESS));
 }
 
diff --git a/secure_fw/spm/cmsis_psa/tfm_nspm_ipc.c b/secure_fw/spm/cmsis_psa/tfm_nspm_ipc.c
deleted file mode 100755
index 0d91a57..0000000
--- a/secure_fw/spm/cmsis_psa/tfm_nspm_ipc.c
+++ /dev/null
@@ -1,21 +0,0 @@
-/*
- * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- *
- */
-
-#include "cmsis.h"
-#include "compiler_ext_defs.h"
-#include "tfm_hal_platform.h"
-
-void configure_ns_code(void)
-{
-    /* SCB_NS.VTOR points to the Non-secure vector table base address */
-    SCB_NS->VTOR = tfm_hal_get_ns_VTOR();
-
-    /* Setups Main stack pointer of the non-secure code */
-    uint32_t ns_msp = tfm_hal_get_ns_MSP();
-
-    __TZ_set_MSP_NS(ns_msp);
-}
diff --git a/secure_fw/spm/ffm/backend_ipc.c b/secure_fw/spm/ffm/backend_ipc.c
index 1572674..0cfbcb5 100644
--- a/secure_fw/spm/ffm/backend_ipc.c
+++ b/secure_fw/spm/ffm/backend_ipc.c
@@ -106,7 +106,6 @@
                                     uint32_t service_setting)
 {
     const struct partition_load_info_t *p_pldi = p_pt->p_ldinf;
-    void *p_param = NULL;
 
     p_pt->signals_allowed |= PSA_DOORBELL | service_setting;
 
@@ -116,19 +115,14 @@
     THRD_INIT(&p_pt->thrd, &p_pt->ctx_ctrl,
               TO_THREAD_PRIORITY(PARTITION_PRIORITY(p_pldi->flags)));
 
+#if defined(CONFIG_TFM_PSA_API_CROSS_CALL) && !defined(TFM_MULTI_CORE_TOPOLOGY)
     if (p_pldi->pid == TFM_SP_NON_SECURE_ID) {
-        p_param = (void *)tfm_hal_get_ns_entry_point();
-
-#ifdef CONFIG_TFM_PSA_API_CROSS_CALL
-#ifndef TFM_MULTI_CORE_TOPOLOGY
         SPM_THREAD_CONTEXT = &p_pt->ctx_ctrl;
-#endif
-#endif
-
     }
+#endif
 
     thrd_start(&p_pt->thrd,
-               POSITION_TO_ENTRY(p_pldi->entry, thrd_fn_t), p_param,
+               POSITION_TO_ENTRY(p_pldi->entry, thrd_fn_t), NULL,
                LOAD_ALLOCED_STACK_ADDR(p_pldi),
                LOAD_ALLOCED_STACK_ADDR(p_pldi) + p_pldi->stack_size);
 }
diff --git a/secure_fw/spm/ffm/backend_sfn.c b/secure_fw/spm/ffm/backend_sfn.c
index a0275fc..3904e0a 100644
--- a/secure_fw/spm/ffm/backend_sfn.c
+++ b/secure_fw/spm/ffm/backend_sfn.c
@@ -104,7 +104,7 @@
 
         thrd_start(&p_pt->thrd,
                POSITION_TO_ENTRY(p_pldi->entry, thrd_fn_t),
-               (void *)tfm_hal_get_ns_entry_point(),
+               NULL,
                LOAD_ALLOCED_STACK_ADDR(p_pldi),
                LOAD_ALLOCED_STACK_ADDR(p_pldi) + p_pldi->stack_size);
 
diff --git a/secure_fw/spm/include/tfm_nspm.h b/secure_fw/spm/include/tfm_nspm.h
index 8b27d6e..7906f80 100644
--- a/secure_fw/spm/include/tfm_nspm.h
+++ b/secure_fw/spm/include/tfm_nspm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -53,23 +53,4 @@
  */
 int32_t tfm_nspm_get_current_client_id(void);
 
-#ifdef TFM_PSA_API
-/**
- * \brief NSPM thread main entry function
- *
- * Note: This function should not return back.
- */
-void tfm_nspm_thread_entry(void);
-#endif
-
-#ifdef TFM_MULTI_CORE_TOPOLOGY
-/* Unnecessary to configure Non-secure side code */
-#define configure_ns_code()               do {} while (0)
-#else
-/*
- * \brief Configure Non-secure code, such as vector table, MSP and entry point.
- */
-void configure_ns_code(void);
-#endif
-
 #endif /* __TFM_NSPM_H__ */