diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/platform/include/tfm_spm_hal.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/platform/include/tfm_spm_hal.h
index 509f153d65..3ce0b29b19 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/platform/include/tfm_spm_hal.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/platform/include/tfm_spm_hal.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2019, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -28,7 +28,7 @@
  */
 struct tfm_spm_partition_platform_data_t;
 
-#if TFM_LVL != 1
+#if defined (TFM_PSA_API) || (TFM_LVL != 1)
 /**
  * \brief Holds SPM db fields that define the memory regions used by a
  *        partition.
@@ -147,7 +147,7 @@ uint32_t tfm_spm_hal_get_ns_MSP(void);
 uint32_t tfm_spm_hal_get_ns_entry_point(void);
 
 
-#if TFM_LVL != 1
+#if (TFM_LVL != 1) && !defined(TFM_PSA_API)
 /**
  * \brief Configure the sandbox for a partition.
  *
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_spm.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_spm.h
index e4df835df8..82c25ebdd1 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_spm.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_spm.h
@@ -75,6 +75,16 @@ struct tfm_spm_ipc_partition_t {
  */
 uint32_t tfm_spm_partition_get_running_partition_id_ext(void);
 
+/**
+ * \brief                   Get the current partition mode.
+ *
+ * \param[in] partition_idx                 Index of current partition
+ *
+ * \retval TFM_PARTITION_PRIVILEGED_MODE    Privileged mode
+ * \retval TFM_PARTITION_UNPRIVILEGED_MODE  Unprivileged mode
+ */
+uint32_t tfm_spm_partition_get_privileged_mode(uint32_t partition_idx);
+
 /******************** Service handle management functions ********************/
 
 /**
@@ -279,22 +289,38 @@ int32_t tfm_spm_check_client_version(struct tfm_spm_service_t *service,
                                      uint32_t minor_version);
 
 /**
- * \brief                   Check the memory reference is valid.
+ * \brief                      Check the memory reference is valid.
  *
- * \param[in] buffer        Pointer of memory reference
- * \param[in] len           Length of memory reference in bytes
- * \param[in] ns_caller     From non-secure caller
- * \param[in] access        Type of access specified by the
- *                          \ref tfm_memory_access_e
+ * \param[in] buffer           Pointer of memory reference
+ * \param[in] len              Length of memory reference in bytes
+ * \param[in] ns_caller        From non-secure caller
+ * \param[in] access           Type of access specified by the
+ *                             \ref tfm_memory_access_e
+ * \param[in] privileged       Privileged mode or unprivileged mode:
+ *                             \ref TFM_PARTITION_UNPRIVILEGED_MODE
+ *                             \ref TFM_PARTITION_PRIVILEGED_MODE
  *
- * \retval IPC_SUCCESS      Success
- * \retval IPC_ERROR_BAD_PARAMETERS Bad parameters input
- * \retval IPC_ERROR_MEMORY_CHECK Check failed
+ * \retval IPC_SUCCESS               Success
+ * \retval IPC_ERROR_BAD_PARAMETERS  Bad parameters input
+ * \retval IPC_ERROR_MEMORY_CHECK    Check failed
  */
 int32_t tfm_memory_check(void *buffer, size_t len, int32_t ns_caller,
-                         enum tfm_memory_access_e access);
+                         enum tfm_memory_access_e access,
+                         uint32_t privileged);
 
 /* This function should be called before schedule function */
 void tfm_spm_init(void);
 
+/*
+ * PendSV specified function.
+ *
+ * Parameters :
+ *  ctxb        -    State context storage pointer
+ *
+ * Notes:
+ *  This is a staging API. Scheduler should be called in SPM finally and
+ *  this function will be obsoleted later.
+ */
+void tfm_pendsv_do_schedule(struct tfm_state_context_ext *ctxb);
+
 #endif
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_thread.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_thread.h
index 8a6d86364d..68aa5a20b0 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_thread.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/include/tfm_thread.h
@@ -177,6 +177,19 @@ struct tfm_thrd_ctx *tfm_thrd_curr_thread(void);
  */
 struct tfm_thrd_ctx *tfm_thrd_next_thread(void);
 
+/*
+ * Start scheduler for existing threads
+ *
+ * Parameters:
+ *  pth         -     pointer of the caller context collecting thread
+ *
+ * Notes :
+ *  This function should be called only ONCE to start the scheduler.
+ *  Caller needs to provide a thread object to collect current context.
+ *  The usage of the collected context is caller defined.
+ */
+void tfm_thrd_start_scheduler(struct tfm_thrd_ctx *pth);
+
 /*
  * Activate a scheduling action after exception.
  *
@@ -201,23 +214,19 @@ void tfm_thrd_context_switch(struct tfm_state_context_ext *ctxb,
                              struct tfm_thrd_ctx *next);
 
 /*
- * Exit current running thread.
+ * Svcall to exit current running thread.
  *
  * Notes :
  *  Remove current thread out of schedulable list.
  */
-void tfm_thrd_do_exit(void);
+void tfm_svcall_thrd_exit(void);
 
 /*
- * PendSV specified function.
- *
- * Parameters :
- *  ctxb        -    State context storage pointer
+ * Exit current running thread for client.
  *
  * Notes:
- *  This is a staging API. Scheduler should be called in SPM finally and
- *  this function will be obsoleted later.
+ *  Must be called in thread mode.
  */
-void tfm_pendsv_do_schedule(struct tfm_state_context_ext *ctxb);
+void tfm_thrd_exit(void);
 
 #endif
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_client.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_client.c
index 8d71af7a8f..6abc9665a1 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_client.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_client.c
@@ -10,7 +10,7 @@
 #include "tfm_svc.h"
 #include "psa_client.h"
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 uint32_t psa_framework_version(void)
 {
     __ASM volatile("SVC %0           \n"
@@ -18,7 +18,7 @@ uint32_t psa_framework_version(void)
                    : : "I" (TFM_SVC_PSA_FRAMEWORK_VERSION));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 uint32_t psa_version(uint32_t sid)
 {
     __ASM volatile("SVC %0           \n"
@@ -26,7 +26,7 @@ uint32_t psa_version(uint32_t sid)
                    : : "I" (TFM_SVC_PSA_VERSION));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 psa_handle_t psa_connect(uint32_t sid, uint32_t minor_version)
 {
     __ASM volatile("SVC %0           \n"
@@ -34,7 +34,7 @@ psa_handle_t psa_connect(uint32_t sid, uint32_t minor_version)
                    : : "I" (TFM_SVC_PSA_CONNECT));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 psa_status_t psa_call(psa_handle_t handle,
                       const psa_invec *in_vec,
                       size_t in_len,
@@ -46,7 +46,7 @@ psa_status_t psa_call(psa_handle_t handle,
                    : : "I" (TFM_SVC_PSA_CALL));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 void psa_close(psa_handle_t handle)
 {
     __ASM volatile("SVC %0           \n"
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_service.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_service.c
index cd2e1cd09f..0c86954d21 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_service.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/psa_service.c
@@ -11,7 +11,7 @@
 #include "psa_client.h"
 #include "psa_service.h"
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 psa_signal_t psa_wait(psa_signal_t signal_mask, uint32_t timeout)
 
 {
@@ -20,7 +20,7 @@ psa_signal_t psa_wait(psa_signal_t signal_mask, uint32_t timeout)
                    : : "I" (TFM_SVC_PSA_WAIT));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 psa_status_t psa_get(psa_signal_t signal, psa_msg_t *msg)
 {
     __ASM volatile("SVC %0           \n"
@@ -28,7 +28,7 @@ psa_status_t psa_get(psa_signal_t signal, psa_msg_t *msg)
                    : : "I" (TFM_SVC_PSA_GET));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 void psa_set_rhandle(psa_handle_t msg_handle, void *rhandle)
 {
     __ASM volatile("SVC %0           \n"
@@ -36,7 +36,7 @@ void psa_set_rhandle(psa_handle_t msg_handle, void *rhandle)
                    : : "I" (TFM_SVC_PSA_SET_RHANDLE));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 size_t psa_read(psa_handle_t msg_handle, uint32_t invec_idx,
                 void *buffer, size_t num_bytes)
 
@@ -46,7 +46,7 @@ size_t psa_read(psa_handle_t msg_handle, uint32_t invec_idx,
                    : : "I" (TFM_SVC_PSA_READ));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 size_t psa_skip(psa_handle_t msg_handle, uint32_t invec_idx, size_t num_bytes)
 {
     __ASM volatile("SVC %0           \n"
@@ -54,7 +54,7 @@ size_t psa_skip(psa_handle_t msg_handle, uint32_t invec_idx, size_t num_bytes)
                    : : "I" (TFM_SVC_PSA_SKIP));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 void psa_write(psa_handle_t msg_handle, uint32_t outvec_idx,
                const void *buffer, size_t num_bytes)
 {
@@ -63,7 +63,7 @@ void psa_write(psa_handle_t msg_handle, uint32_t outvec_idx,
                    : : "I" (TFM_SVC_PSA_WRITE));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 void psa_reply(psa_handle_t msg_handle, psa_status_t retval)
 {
     __ASM volatile("SVC %0           \n"
@@ -71,7 +71,7 @@ void psa_reply(psa_handle_t msg_handle, psa_status_t retval)
                    : : "I" (TFM_SVC_PSA_REPLY));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 void psa_notify(int32_t partition_id)
 {
     __ASM volatile("SVC %0           \n"
@@ -79,7 +79,7 @@ void psa_notify(int32_t partition_id)
                    : : "I" (TFM_SVC_PSA_NOTIFY));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 void psa_clear(void)
 {
     __ASM volatile("SVC %0           \n"
@@ -87,7 +87,7 @@ void psa_clear(void)
                    : : "I" (TFM_SVC_PSA_CLEAR));
 }
 
-__attribute__((naked))
+__attribute__((naked, section("SFN")))
 void psa_eoi(psa_signal_t irq_signal)
 {
     __ASM volatile("SVC %0           \n"
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_arch_v8m.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_arch_v8m.c
index 137c3e75b6..ebe1c45a6d 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_arch_v8m.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_arch_v8m.c
@@ -17,19 +17,10 @@
 
 /* This file contains the ARCH code for ARM V8M */
 
-/*
- * Thread exit zone.
- * This function is set as the return address of thread entry and only
- * privileged thread could return here. Un-privileged thread triggers
- * fault if it tries to jump here and it gets exit by fault handler.
- *
- * The reason of putting this function here is for fault handler checking.
- * Function address could be checked in fault handler to know it is a REAL
- * thread exit or just an exception.
- */
+__attribute__((section("SFN")))
 static void exit_zone(void)
 {
-    tfm_thrd_do_exit();
+    tfm_thrd_exit();
 }
 
 void tfm_initialize_context(struct tfm_state_context *ctx,
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_spm.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_spm.c
index fe82120a04..30210d940b 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_spm.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_spm.c
@@ -38,6 +38,8 @@ static struct tfm_spm_ipc_partition_t
 /* Extern SPM variable */
 extern struct spm_partition_db_t g_spm_partition_db;
 
+/* Extern secure lock variable */
+extern int32_t tfm_secure_lock;
 /* Pools */
 TFM_POOL_DECLARE(conn_handle_pool, sizeof(struct tfm_conn_handle_t),
                  TFM_CONN_HANDLE_MAX_NUM);
@@ -433,21 +435,6 @@ tfm_spm_partition_get_thread_info_ext(uint32_t partition_idx)
     return &g_spm_partition_db.partitions[partition_idx].sp_thrd;
 }
 
-static uint32_t tfm_spm_partition_get_stack_size_ext(uint32_t partition_idx)
-{
-    return g_spm_partition_db.partitions[partition_idx].stack_size;
-}
-
-static uint32_t tfm_spm_partition_get_stack_limit_ext(uint32_t partition_idx)
-{
-    return g_spm_partition_db.partitions[partition_idx].stack_limit;
-}
-
-static uint32_t tfm_spm_partition_get_stack_base_ext(uint32_t partition_idx)
-{
-    return tfm_spm_partition_get_stack_limit_ext(partition_idx) + tfm_spm_partition_get_stack_size_ext(partition_idx);
-}
-
 static tfm_thrd_func_t
         tfm_spm_partition_get_init_func_ext(uint32_t partition_idx)
 {
@@ -461,9 +448,9 @@ static uint32_t tfm_spm_partition_get_priority_ext(uint32_t partition_idx)
                     partition_priority;
 }
 
-/* FixMe: This is only valid for TFM LVL 1 now */
 int32_t tfm_memory_check(void *buffer, size_t len, int32_t ns_caller,
-                         enum tfm_memory_access_e access)
+                         enum tfm_memory_access_e access,
+                         uint32_t privileged)
 {
     int32_t err;
 
@@ -481,17 +468,28 @@ int32_t tfm_memory_check(void *buffer, size_t len, int32_t ns_caller,
     }
 
     if (access == TFM_MEMORY_ACCESS_RW) {
-        err = tfm_core_has_write_access_to_region(buffer, len, ns_caller);
+        err = tfm_core_has_write_access_to_region(buffer, len, ns_caller,
+                                                  privileged);
     } else {
-        err = tfm_core_has_read_access_to_region(buffer, len, ns_caller);
+        err = tfm_core_has_read_access_to_region(buffer, len, ns_caller,
+                                                 privileged);
     }
-    if (err == 1) {
+    if (err == TFM_SUCCESS) {
         return IPC_SUCCESS;
     }
 
     return IPC_ERROR_MEMORY_CHECK;
 }
 
+uint32_t tfm_spm_partition_get_privileged_mode(uint32_t partition_idx)
+{
+    if (tfm_spm_partition_get_flags(partition_idx) & SPM_PART_FLAG_PSA_ROT) {
+        return TFM_PARTITION_PRIVILEGED_MODE;
+    } else {
+        return TFM_PARTITION_UNPRIVILEGED_MODE;
+    }
+}
+
 /********************** SPM functions for thread mode ************************/
 
 void tfm_spm_init(void)
@@ -499,7 +497,8 @@ void tfm_spm_init(void)
     uint32_t i, num;
     struct tfm_spm_ipc_partition_t *partition;
     struct tfm_spm_service_t *service;
-    struct tfm_thrd_ctx *pth;
+    struct tfm_thrd_ctx *pth, this_thrd;
+    struct spm_partition_desc_t *part;
 
     tfm_pool_init(conn_handle_pool,
                   POOL_BUFFER_SIZE(conn_handle_pool),
@@ -514,10 +513,12 @@ void tfm_spm_init(void)
 
     /* Init partition first for it will be used when init service */
     for (i = 0; i < SPM_MAX_PARTITIONS; i++) {
+        part = &g_spm_partition_db.partitions[i];
+        tfm_spm_hal_configure_default_isolation(part->platform_data);
+        g_spm_ipc_partition[i].index = i;
         if ((tfm_spm_partition_get_flags(i) & SPM_PART_FLAG_IPC) == 0) {
             continue;
         }
-        g_spm_ipc_partition[i].index = i;
         g_spm_ipc_partition[i].id = tfm_spm_partition_get_partition_id(i);
 
         tfm_event_init(&g_spm_ipc_partition[i].signal_evnt);
@@ -531,8 +532,9 @@ void tfm_spm_init(void)
         tfm_thrd_init(pth,
                       tfm_spm_partition_get_init_func_ext(i),
                       NULL,
-                      (uint8_t *)tfm_spm_partition_get_stack_base_ext(i),
-                      (uint8_t *)tfm_spm_partition_get_stack_limit_ext(i));
+                      (uint8_t *)tfm_spm_partition_get_stack_top(i),
+                      (uint8_t *)tfm_spm_partition_get_stack_bottom(i));
+
         pth->prior = tfm_spm_partition_get_priority_ext(i);
 
         /* Kick off */
@@ -559,6 +561,58 @@ void tfm_spm_init(void)
         tfm_list_add_tail(&partition->service_list, &service->list);
     }
 
-    /* All thread inited.... trigger scheduler */
-    tfm_thrd_activate_schedule();
+    /*
+     * All threads initialized, start the scheduler.
+     *
+     * NOTE:
+     * Here is the booting privileged thread mode, and will never
+     * return to this place after scheduler is started. The start
+     * function has to save current runtime context to act as a
+     * 'current thread' to avoid repeating NULL 'current thread'
+     * checking while context switching. This saved context is worthy
+     * of being saved somewhere if there are potential usage purpose.
+     * Let's save this context in a local variable 'this_thrd' at
+     * current since there is no usage for it.
+     * Also set tfm_nspm_thread_entry as pfn for this thread to
+     * use in detecting NS/S thread scheduling changes.
+     */
+    this_thrd.pfn = (tfm_thrd_func_t)tfm_nspm_thread_entry;
+    tfm_thrd_start_scheduler(&this_thrd);
+}
+
+void tfm_pendsv_do_schedule(struct tfm_state_context_ext *ctxb)
+{
+#if TFM_LVL == 2
+    struct spm_partition_desc_t *p_next_partition;
+    uint32_t is_privileged;
+#endif
+    struct tfm_thrd_ctx *pth_next = tfm_thrd_next_thread();
+    struct tfm_thrd_ctx *pth_curr = tfm_thrd_curr_thread();
+
+    if (pth_curr != pth_next) {
+#if TFM_LVL == 2
+        p_next_partition = TFM_GET_CONTAINER_PTR(pth_next,
+                                                 struct spm_partition_desc_t,
+                                                 sp_thrd);
+
+        if (p_next_partition->static_data.partition_flags &
+            SPM_PART_FLAG_PSA_ROT) {
+            is_privileged = TFM_PARTITION_PRIVILEGED_MODE;
+        } else {
+            is_privileged = TFM_PARTITION_UNPRIVILEGED_MODE;
+        }
+
+        tfm_spm_partition_change_privilege(is_privileged);
+#endif
+        /* Increase the secure lock, if we enter secure from non-secure */
+        if ((void *)pth_curr->pfn == (void *)tfm_nspm_thread_entry) {
+            ++tfm_secure_lock;
+        }
+        /* Decrease the secure lock, if we return from secure to non-secure */
+        if ((void *)pth_next->pfn == (void *)tfm_nspm_thread_entry) {
+            --tfm_secure_lock;
+        }
+
+        tfm_thrd_context_switch(ctxb, pth_curr, pth_next);
+    }
 }
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c
index 29f9130499..eefc031d04 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_svcalls.c
@@ -21,6 +21,7 @@
 #include "tfm_api.h"
 #include "tfm_secure_api.h"
 #include "tfm_memory_utils.h"
+#include "spm_api.h"
 
 #define PSA_TIMEOUT_MASK        PSA_BLOCK
 
@@ -116,9 +117,18 @@ psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr)
     struct tfm_spm_service_t *service;
     struct tfm_msg_body_t *msg;
     int i;
+    struct tfm_spm_ipc_partition_t *partition = NULL;
+    uint32_t privileged;
 
     TFM_ASSERT(args != NULL);
     handle = (psa_handle_t)args[0];
+
+    partition = tfm_spm_get_running_partition();
+    if (!partition) {
+        tfm_panic();
+    }
+    privileged = tfm_spm_partition_get_privileged_mode(partition->index);
+
     if (!ns_caller) {
         inptr = (psa_invec *)args[1];
         in_num = (size_t)args[2];
@@ -153,11 +163,11 @@ psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr)
          * memory reference for buffer is invalid or not readable.
          */
         if (tfm_memory_check((void *)args[1], sizeof(uint32_t),
-            ns_caller, TFM_MEMORY_ACCESS_RO) != IPC_SUCCESS) {
+            ns_caller, TFM_MEMORY_ACCESS_RO, privileged) != IPC_SUCCESS) {
             tfm_panic();
         }
         if (tfm_memory_check((void *)args[2], sizeof(uint32_t),
-            ns_caller, TFM_MEMORY_ACCESS_RO) != IPC_SUCCESS) {
+            ns_caller, TFM_MEMORY_ACCESS_RO, privileged) != IPC_SUCCESS) {
             tfm_panic();
         }
 
@@ -185,7 +195,7 @@ psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr)
      * readable.
      */
     if (tfm_memory_check((void *)inptr, in_num * sizeof(psa_invec),
-        ns_caller, TFM_MEMORY_ACCESS_RO) != IPC_SUCCESS) {
+        ns_caller, TFM_MEMORY_ACCESS_RO, privileged) != IPC_SUCCESS) {
         tfm_panic();
     }
     /*
@@ -194,7 +204,7 @@ psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr)
      * the wrap output vector is invalid or not read-write.
      */
     if (tfm_memory_check((void *)outptr, out_num * sizeof(psa_outvec),
-        ns_caller, TFM_MEMORY_ACCESS_RW) != IPC_SUCCESS) {
+        ns_caller, TFM_MEMORY_ACCESS_RW, privileged) != IPC_SUCCESS) {
         tfm_panic();
     }
 
@@ -211,7 +221,7 @@ psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr)
      */
     for (i = 0; i < in_num; i++) {
         if (tfm_memory_check((void *)invecs[i].base, invecs[i].len,
-            ns_caller, TFM_MEMORY_ACCESS_RO) != IPC_SUCCESS) {
+            ns_caller, TFM_MEMORY_ACCESS_RO, privileged) != IPC_SUCCESS) {
             tfm_panic();
         }
     }
@@ -221,7 +231,7 @@ psa_status_t tfm_svcall_psa_call(uint32_t *args, int32_t ns_caller, uint32_t lr)
      */
     for (i = 0; i < out_num; i++) {
         if (tfm_memory_check(outvecs[i].base, outvecs[i].len,
-            ns_caller, TFM_MEMORY_ACCESS_RW) != IPC_SUCCESS) {
+            ns_caller, TFM_MEMORY_ACCESS_RW, privileged) != IPC_SUCCESS) {
             tfm_panic();
         }
     }
@@ -363,6 +373,7 @@ static psa_status_t tfm_svcall_psa_get(uint32_t *args)
     struct tfm_spm_service_t *service = NULL;
     struct tfm_msg_body_t *tmp_msg = NULL;
     struct tfm_spm_ipc_partition_t *partition = NULL;
+    uint32_t privileged;
 
     TFM_ASSERT(args != NULL);
     signal = (psa_signal_t)args[0];
@@ -376,17 +387,18 @@ static psa_status_t tfm_svcall_psa_get(uint32_t *args)
         tfm_panic();
     }
 
+    partition = tfm_spm_get_running_partition();
+    if (!partition) {
+        tfm_panic();
+    }
+    privileged = tfm_spm_partition_get_privileged_mode(partition->index);
+
     /*
      * Write the message to the service buffer. It is a fatal error if the
      * input msg pointer is not a valid memory reference or not read-write.
      */
     if (tfm_memory_check((void *)msg, sizeof(psa_msg_t),
-        false, TFM_MEMORY_ACCESS_RW) != IPC_SUCCESS) {
-        tfm_panic();
-    }
-
-    partition = tfm_spm_get_running_partition();
-    if (!partition) {
+        false, TFM_MEMORY_ACCESS_RW, privileged) != IPC_SUCCESS) {
         tfm_panic();
     }
 
@@ -499,6 +511,8 @@ static size_t tfm_svcall_psa_read(uint32_t *args)
     size_t num_bytes;
     size_t bytes;
     struct tfm_msg_body_t *msg = NULL;
+    uint32_t privileged;
+    struct tfm_spm_ipc_partition_t *partition = NULL;
 
     TFM_ASSERT(args != NULL);
     msg_handle = (psa_handle_t)args[0];
@@ -512,6 +526,9 @@ static size_t tfm_svcall_psa_read(uint32_t *args)
         tfm_panic();
     }
 
+    partition = msg->service->partition;
+    privileged = tfm_spm_partition_get_privileged_mode(partition->index);
+
     /*
      * It is a fatal error if message handle does not refer to a PSA_IPC_CALL
      * message
@@ -538,7 +555,7 @@ static size_t tfm_svcall_psa_read(uint32_t *args)
      * if the memory reference for buffer is invalid or not read-write.
      */
     if (tfm_memory_check(buffer, num_bytes, false,
-        TFM_MEMORY_ACCESS_RW) != IPC_SUCCESS) {
+        TFM_MEMORY_ACCESS_RW, privileged) != IPC_SUCCESS) {
         tfm_panic();
     }
 
@@ -650,6 +667,8 @@ static void tfm_svcall_psa_write(uint32_t *args)
     void *buffer = NULL;
     size_t num_bytes;
     struct tfm_msg_body_t *msg = NULL;
+    uint32_t privileged;
+    struct tfm_spm_ipc_partition_t *partition = NULL;
 
     TFM_ASSERT(args != NULL);
     msg_handle = (psa_handle_t)args[0];
@@ -663,6 +682,9 @@ static void tfm_svcall_psa_write(uint32_t *args)
         tfm_panic();
     }
 
+    partition = msg->service->partition;
+    privileged = tfm_spm_partition_get_privileged_mode(partition->index);
+
     /*
      * It is a fatal error if message handle does not refer to a PSA_IPC_CALL
      * message
@@ -693,7 +715,7 @@ static void tfm_svcall_psa_write(uint32_t *args)
      * if the memory reference for buffer is invalid or not readable.
      */
     if (tfm_memory_check(buffer, num_bytes, false,
-        TFM_MEMORY_ACCESS_RO) != IPC_SUCCESS) {
+        TFM_MEMORY_ACCESS_RO, privileged) != IPC_SUCCESS) {
         tfm_panic();
     }
 
@@ -957,6 +979,9 @@ int32_t SVC_Handler_IPC(tfm_svc_number_t svc_num, uint32_t *ctx, uint32_t lr)
     case TFM_SVC_SCHEDULE:
         tfm_thrd_activate_schedule();
         break;
+    case TFM_SVC_EXIT_THRD:
+        tfm_svcall_thrd_exit();
+        break;
     case TFM_SVC_PSA_FRAMEWORK_VERSION:
         return tfm_svcall_psa_framework_version();
     case TFM_SVC_PSA_VERSION:
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_thread.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_thread.c
index cd01259e6e..21f22091a7 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_thread.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/ipc/tfm_thread.c
@@ -10,6 +10,8 @@
 #include "tfm_thread.h"
 #include "tfm_utils.h"
 #include "tfm_memory_utils.h"
+#include "tfm_svc.h"
+#include "spm_api.h"
 
 /* Force ZERO in case ZI(bss) clear is missing */
 static struct tfm_thrd_ctx *p_thrd_head = NULL;
@@ -124,75 +126,56 @@ void tfm_thrd_set_status(struct tfm_thrd_ctx *pth, uint32_t new_status)
     update_running_head(&RUNN_HEAD, pth);
 }
 
-/*
- * TEMP WORKAROUND: The caller function who called thread module init needs to
- * be returned. The caller is not a thread. Create a dummy IDLE thread to
- * collect caller context; and schedule back to the caller with this context
- * after all other real threads blocked.
- *
- * This WORKAROUND needs to be removed after IPC NSPM takes place.
- */
-#define DUMMY_IDLE_TAG       0xDEEDDEED
-static uint8_t idle_stack[32] __attribute__((aligned(8)));
-static struct tfm_thrd_ctx idle_thread;
-static struct tfm_thrd_ctx *init_idle_thread(struct tfm_thrd_ctx *pth)
-{
-    /*
-     * IDLE thread is a thread with the lowest priority.
-     * It gets scheduled after all other higher priority threads get blocked.
-     * The entry of IDLE thread is a dummy and has no mean.
-     */
-    tfm_thrd_init(pth, (tfm_thrd_func_t)DUMMY_IDLE_TAG, NULL,
-                  (uint8_t *)&idle_stack[32], (uint8_t *)idle_stack);
-    tfm_thrd_priority(pth, THRD_PRIOR_LOWEST);
-    tfm_thrd_start(pth);
-    return pth;
-}
-
 /* Scheduling won't happen immediately but after the exception returns */
 void tfm_thrd_activate_schedule(void)
 {
-    /*
-     * The current thread can be NULL only when initializing. Create the IDLE
-     * thread and set it as the current thread to collect caller context.
-     */
-    if (CURR_THRD == NULL) {
-        CURR_THRD = init_idle_thread(&idle_thread);
-    }
-
     tfm_trigger_pendsv();
 }
 
+void tfm_thrd_start_scheduler(struct tfm_thrd_ctx *pth)
+{
+    /*
+     * There is no selected thread before scheduler start, assign
+     * a caller provided thread as current thread. This function
+     * should get called only ONCE; further calling triggers assert.
+     */
+    TFM_ASSERT(CURR_THRD == NULL);
+    TFM_ASSERT(pth != NULL);
+
+    CURR_THRD = pth;
+    tfm_thrd_activate_schedule();
+}
+
 /* Remove current thread out of the schedulable list */
-void tfm_thrd_do_exit(void)
+void tfm_svcall_thrd_exit(void)
 {
     CURR_THRD->status = THRD_STAT_DETACH;
     tfm_trigger_pendsv();
 }
 
+__attribute__((section("SFN")))
+void tfm_thrd_exit(void)
+{
+    SVC(TFM_SVC_EXIT_THRD);
+    while (1) {
+        ;
+    }
+}
+
 void tfm_thrd_context_switch(struct tfm_state_context_ext *ctxb,
                              struct tfm_thrd_ctx *prev,
                              struct tfm_thrd_ctx *next)
 {
-    /* Update latest context into the current thread context */
+    TFM_ASSERT(prev != NULL);
+    TFM_ASSERT(next != NULL);
+
+    /*
+     * First, update latest context into the current thread context.
+     * Then, update background context with next thread's context.
+     */
     tfm_memcpy(&prev->state_ctx.ctxb, ctxb, sizeof(*ctxb));
-    /* Update background context with next thread's context */
     tfm_memcpy(ctxb, &next->state_ctx.ctxb, sizeof(next->state_ctx.ctxb));
-    /* Set current thread indicator with next thread */
+
+    /* Update current thread indicator */
     CURR_THRD = next;
 }
-
-/*
- * This function is a reference implementation for PendSV handler in
- * isolation level 1. More jobs (sandboxing e.g.) need to be done while
- * scheduling in other isolation levels.
- */
-void tfm_pendsv_do_schedule(struct tfm_state_context_ext *ctxb)
-{
-    struct tfm_thrd_ctx *pth = tfm_thrd_next_thread();
-
-    /* Swith context if another thread ready to run */
-    if (pth && pth != CURR_THRD) {
-        tfm_thrd_context_switch(ctxb, CURR_THRD, pth);
-    }
-}
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_boot_data.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_boot_data.c
index 3ec8b8e7fd..44c38a35ca 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_boot_data.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_boot_data.c
@@ -12,6 +12,15 @@
 #include "tfm_api.h"
 #include "flash_layout.h"
 #include "secure_fw/spm/spm_api.h"
+#ifdef TFM_PSA_API
+#include "tfm_internal_defines.h"
+#include "tfm_utils.h"
+#include "psa_service.h"
+#include "tfm_thread.h"
+#include "tfm_wait.h"
+#include "tfm_message_queue.h"
+#include "tfm_spm.h"
+#endif
 
 /*!
  * \def BOOT_DATA_VALID
@@ -60,13 +69,19 @@ void tfm_core_get_boot_data_handler(uint32_t args[])
     uint8_t *buf_start = (uint8_t *)args[1];
     uint16_t buf_size  = (uint16_t)args[2];
     uint8_t *ptr;
-    uint32_t running_partition_idx =
-            tfm_spm_partition_get_running_partition_idx();
     struct tfm_boot_data *boot_data;
     struct shared_data_tlv_entry tlv_entry;
     uintptr_t tlv_end, offset;
+#ifndef TFM_PSA_API
+    uint32_t running_partition_idx =
+                tfm_spm_partition_get_running_partition_idx();
     uint32_t res;
+#else
+    struct tfm_spm_ipc_partition_t *partition = NULL;
+    uint32_t privileged;
+#endif
 
+#ifndef TFM_PSA_API
     /* Make sure that the output pointer points to a memory area that is owned
      * by the partition
      */
@@ -79,6 +94,20 @@ void tfm_core_get_boot_data_handler(uint32_t args[])
         args[0] = TFM_ERROR_INVALID_PARAMETER;
         return;
     }
+#else
+    partition = tfm_spm_get_running_partition();
+    if (!partition) {
+        tfm_panic();
+    }
+    privileged = tfm_spm_partition_get_privileged_mode(partition->index);
+
+    if (tfm_memory_check(buf_start, buf_size, false, TFM_MEMORY_ACCESS_RW,
+        privileged) != IPC_SUCCESS) {
+        /* Not in accessible range, return error */
+        args[0] = TFM_ERROR_INVALID_PARAMETER;
+        return;
+    }
+#endif
 
     /* FixMe: Check whether caller has access right to given tlv_major_type */
 
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_core.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_core.c
index 7d831b5451..0be05ca8f1 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_core.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_core.c
@@ -47,6 +47,7 @@ __asm("  .global __ARM_use_no_argv\n");
 #error Only TFM_LVL 1, 2 and 3 are supported!
 #endif
 
+#ifndef TFM_PSA_API
 /* Macros to pick linker symbols and allow to form the partition data base */
 #define REGION(a, b, c) a##b##c
 #define REGION_NAME(a, b, c) REGION(a, b, c)
@@ -54,6 +55,7 @@ __asm("  .global __ARM_use_no_argv\n");
 
 REGION_DECLARE(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Base);
 REGION_DECLARE(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Limit);
+#endif
 
 void configure_ns_code(void)
 {
@@ -106,11 +108,20 @@ int32_t tfm_core_init(void)
     /* Enable secure peripherals interrupts */
     nvic_interrupt_enable();
 
+#ifdef TFM_PSA_API
+    /* FixMe: In case of IPC messaging, scratch area must not be referenced
+     * These variables should be removed when all obsolete references are
+     * removed from the codebase
+     */
+    tfm_scratch_area = NULL;
+    tfm_scratch_area_size = 0;
+#else
     tfm_scratch_area =
         (uint8_t *)&REGION_NAME(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Base);
     tfm_scratch_area_size =
         (uint32_t)&REGION_NAME(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Limit) -
         (uint32_t)&REGION_NAME(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Base);
+#endif
     return 0;
 }
 
@@ -179,6 +190,7 @@ int main(void)
 
     tfm_spm_hal_setup_isolation_hw();
 
+#ifndef TFM_PSA_API
     tfm_spm_partition_set_state(TFM_SP_CORE_ID, SPM_PARTITION_STATE_RUNNING);
 
     extern uint32_t Image$$ARM_LIB_STACK$$ZI$$Base[];
@@ -198,21 +210,25 @@ int main(void)
      */
     tfm_core_set_secure_exception_priorities();
 
-#ifdef TFM_PSA_API
-    tfm_spm_init();
-#endif
+    /* We close the TFM_SP_CORE_ID partition, because its only purpose is
+     * to be able to pass the state checks for the tests started from secure.
+     */
+    tfm_spm_partition_set_state(TFM_SP_CORE_ID, SPM_PARTITION_STATE_CLOSED);
+    tfm_spm_partition_set_state(TFM_SP_NON_SECURE_ID,
+                                SPM_PARTITION_STATE_RUNNING);
 
 #ifdef TFM_CORE_DEBUG
     /* Jumps to non-secure code */
     LOG_MSG("Jumping to non-secure code...");
 #endif
 
-    /* We close the TFM_SP_CORE_ID partition, because its only purpose is
-     * to be able to pass the state checks for the tests started from secure.
-     */
-    tfm_spm_partition_set_state(TFM_SP_CORE_ID, SPM_PARTITION_STATE_CLOSED);
-    tfm_spm_partition_set_state(TFM_SP_NON_SECURE_ID,
-                              SPM_PARTITION_STATE_RUNNING);
-
     jump_to_ns_code();
+#else
+    /*
+     * Prioritise secure exceptions to avoid NS being able to pre-empt
+     * secure SVC or SecureFault. Do it before PSA API initialization.
+     */
+    tfm_core_set_secure_exception_priorities();
+    tfm_spm_init();
+#endif
 }
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c
index ef07a13ac7..10a91a56dd 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_handler.c
@@ -156,6 +156,30 @@ uint32_t SVCHandler_main(uint32_t *svc_args, uint32_t lr)
         return lr;
     }
     switch (svc_number) {
+#ifdef TFM_PSA_API
+    case TFM_SVC_IPC_REQUEST:
+        tfm_psa_ipc_request_handler(svc_args);
+        break;
+    case TFM_SVC_SCHEDULE:
+    case TFM_SVC_EXIT_THRD:
+    case TFM_SVC_PSA_FRAMEWORK_VERSION:
+    case TFM_SVC_PSA_VERSION:
+    case TFM_SVC_PSA_CONNECT:
+    case TFM_SVC_PSA_CALL:
+    case TFM_SVC_PSA_CLOSE:
+    case TFM_SVC_PSA_WAIT:
+    case TFM_SVC_PSA_GET:
+    case TFM_SVC_PSA_SET_RHANDLE:
+    case TFM_SVC_PSA_READ:
+    case TFM_SVC_PSA_SKIP:
+    case TFM_SVC_PSA_WRITE:
+    case TFM_SVC_PSA_REPLY:
+    case TFM_SVC_PSA_NOTIFY:
+    case TFM_SVC_PSA_CLEAR:
+    case TFM_SVC_PSA_EOI:
+        svc_args[0] = SVC_Handler_IPC(svc_number, svc_args, lr);
+        break;
+#else
     case TFM_SVC_SFN_REQUEST:
         lr = tfm_core_partition_request_svc_handler(svc_args, lr);
         break;
@@ -177,10 +201,6 @@ uint32_t SVCHandler_main(uint32_t *svc_args, uint32_t lr)
     case TFM_SVC_SET_SHARE_AREA:
         tfm_core_set_buffer_area_handler(svc_args);
         break;
-#ifdef TFM_PSA_API
-    case TFM_SVC_IPC_REQUEST:
-        tfm_psa_ipc_request_handler(svc_args);
-        break;
 #endif
     case TFM_SVC_PRINT:
         printf("\e[1;34m[Sec Thread] %s\e[0m\r\n", (char *)svc_args[0]);
@@ -188,25 +208,6 @@ uint32_t SVCHandler_main(uint32_t *svc_args, uint32_t lr)
     case TFM_SVC_GET_BOOT_DATA:
         tfm_core_get_boot_data_handler(svc_args);
         break;
-#ifdef TFM_PSA_API
-    case TFM_SVC_PSA_FRAMEWORK_VERSION:
-    case TFM_SVC_PSA_VERSION:
-    case TFM_SVC_PSA_CONNECT:
-    case TFM_SVC_PSA_CALL:
-    case TFM_SVC_PSA_CLOSE:
-    case TFM_SVC_PSA_WAIT:
-    case TFM_SVC_PSA_GET:
-    case TFM_SVC_PSA_SET_RHANDLE:
-    case TFM_SVC_PSA_READ:
-    case TFM_SVC_PSA_SKIP:
-    case TFM_SVC_PSA_WRITE:
-    case TFM_SVC_PSA_REPLY:
-    case TFM_SVC_PSA_NOTIFY:
-    case TFM_SVC_PSA_CLEAR:
-    case TFM_SVC_PSA_EOI:
-        svc_args[0] = SVC_Handler_IPC(svc_number, svc_args, lr);
-        break;
-#endif
     default:
         LOG_MSG("Unknown SVC number requested!");
         break;
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.c
index 3188e2831d..027726585c 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.c
@@ -6,8 +6,13 @@
  */
 
 #include <stdio.h>
+#include <stdbool.h>
 #include "secure_utilities.h"
 #include "tfm_api.h"
+#ifdef TFM_PSA_API
+#include "tfm_utils.h"
+#include "tfm_internal.h"
+#endif
 
 #ifndef TFM_MAX_NS_THREAD_COUNT
 #define TFM_MAX_NS_THREAD_COUNT 8
@@ -300,3 +305,20 @@ enum tfm_status_e tfm_register_client_id (int32_t ns_client_id)
     return TFM_SUCCESS;
 }
 #endif
+
+#ifdef TFM_PSA_API
+__attribute__((section("SFN")))
+psa_status_t tfm_nspm_thread_entry(void)
+{
+#ifdef TFM_CORE_DEBUG
+    /* Jumps to non-secure code */
+    LOG_MSG("Jumping to non-secure code...");
+#endif
+
+    jump_to_ns_code();
+
+    /* Should not run here */
+    TFM_ASSERT(false);
+    return PSA_SUCCESS;
+}
+#endif
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.h
index b485462519..572c734c0f 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_nspm.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2019, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -23,4 +23,15 @@ void tfm_nspm_configure_clients(void);
  */
 int32_t tfm_nspm_get_current_client_id(void);
 
+#ifdef TFM_PSA_API
+/**
+ * \brief NSPM thread main entry function
+ *
+ * \return  PSA_SUCCESS indicates failed.
+ *
+ * Note: This function should not return back.
+ */
+psa_status_t tfm_nspm_thread_entry(void);
+#endif
+
 #endif /* __TFM_NSPM_H__ */
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.c
index affabbdd13..951765a163 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.c
@@ -27,89 +27,15 @@
 #define REGION_NAME(a, b, c) REGION(a, b, c)
 #define REGION_DECLARE(a, b, c) extern uint32_t REGION_NAME(a, b, c)
 
+#ifndef TFM_PSA_API /* Only use scratch if using veneer functions, not IPC */
 REGION_DECLARE(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Base);
 REGION_DECLARE(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Limit);
-
-#if TFM_LVL == 1
-REGION_DECLARE(Image$$, TFM_SECURE_STACK, $$ZI$$Base);
-REGION_DECLARE(Image$$, TFM_SECURE_STACK, $$ZI$$Limit);
-#endif
+#endif /* !defined(TFM_PSA_API) */
 
 /* This is the "Big Lock" on the secure side, to guarantee single entry
  * to SPE
  */
 int32_t tfm_secure_lock;
-static int32_t tfm_secure_api_initializing = 1;
-
-static int32_t is_iovec_api_call(void)
-{
-    uint32_t current_partition_idx =
-            tfm_spm_partition_get_running_partition_idx();
-    const struct spm_partition_runtime_data_t *curr_part_data =
-            tfm_spm_partition_get_runtime_data(current_partition_idx);
-    return curr_part_data->iovec_api;
-}
-
-static uint32_t *prepare_partition_ctx(
-            const struct tfm_exc_stack_t *svc_ctx,
-            const struct tfm_sfn_req_s *desc_ptr,
-            uint32_t *dst)
-{
-    /* XPSR  = as was when called, but make sure it's thread mode */
-    *(--dst) = svc_ctx->XPSR & 0xFFFFFE00U;
-    /* ReturnAddress = resume veneer in new context */
-    *(--dst) = svc_ctx->RetAddr;
-    /* LR = sfn address */
-    *(--dst) = (uint32_t)desc_ptr->sfn;
-    /* R12 = don't care */
-    *(--dst) = 0;
-
-    /* R0-R3 = sfn arguments */
-    int32_t i = 4;
-
-    while (i > 0) {
-        i--;
-        *(--dst) = (uint32_t)desc_ptr->args[i];
-    }
-    return dst;
-}
-
-static uint32_t *prepare_partition_iovec_ctx(
-                             const struct tfm_exc_stack_t *svc_ctx,
-                             const struct tfm_sfn_req_s *desc_ptr,
-                             const struct iovec_args_t *iovec_args,
-                             uint32_t *dst)
-{
-    /* XPSR  = as was when called, but make sure it's thread mode */
-    *(--dst) = svc_ctx->XPSR & 0xFFFFFE00U;
-    /* ReturnAddress = resume veneer in new context */
-    *(--dst) = svc_ctx->RetAddr;
-    /* LR = sfn address */
-    *(--dst) = (uint32_t)desc_ptr->sfn;
-    /* R12 = don't care */
-    *(--dst) = 0U;
-
-    /* R0-R3 = sfn arguments */
-    *(--dst) = iovec_args->out_len;
-    *(--dst) = (uint32_t)iovec_args->out_vec;
-    *(--dst) = iovec_args->in_len;
-    *(--dst) = (uint32_t)iovec_args->in_vec;
-
-    return dst;
-}
-
-static void restore_caller_ctx(
-            const struct tfm_exc_stack_t *svc_ctx,
-            struct tfm_exc_stack_t *target_ctx)
-{
-    /* ReturnAddress = resume veneer after second SVC */
-    target_ctx->RetAddr = svc_ctx->RetAddr;
-
-    /* R0 = function return value */
-    target_ctx->R0 = svc_ctx->R0;
-
-    return;
-}
 
 /**
  * \brief Check whether a memory range is inside a memory region.
@@ -121,16 +47,17 @@ static void restore_caller_ctx(
  * \param[in] region_len    The size of the region, which should contain the
  *                          range
  *
- * \return 1 if the region contains the range, 0 otherwise.
+ * \return TFM_SUCCESS if the region contains the range,
+ *         TFM_ERROR_GENERIC otherwise.
  */
 static int32_t check_address_range(const void *p, size_t s,
                                    uintptr_t region_start, uint32_t region_len)
 {
-    int32_t range_in_region = 0;
+    int32_t range_in_region;
 
     /* Check for overflow in the range parameters */
     if ((uintptr_t)p > UINTPTR_MAX-s) {
-        return 0;
+        return TFM_ERROR_GENERIC;
     }
 
     /* We trust the region parameters, and don't check for overflow */
@@ -138,8 +65,11 @@ static int32_t check_address_range(const void *p, size_t s,
     /* Calculate the result */
     range_in_region = ((uintptr_t)p >= region_start) &&
                       ((uintptr_t)p+s <= region_start+region_len);
-
-    return range_in_region;
+    if (range_in_region) {
+        return TFM_SUCCESS;
+    } else {
+        return TFM_ERROR_GENERIC;
+    }
 }
 
 /**
@@ -155,47 +85,62 @@ static int32_t check_address_range(const void *p, size_t s,
  * \param[in] s      The size of the range to check
  * \param[in] flags  The flags to pass to the cmse_check_address_range func
  *
- * \return 1 if the partition has access to the memory range, 0 otherwise.
+ * \return TFM_SUCCESS if the partition has access to the memory range,
+ *         TFM_ERROR_GENERIC otherwise.
  */
 static int32_t has_access_to_region(const void *p, size_t s, int flags)
 {
     int32_t range_access_allowed_by_mpu;
 
+#ifndef TFM_PSA_API /* Only use scratch if using veneer functions, not IPC */
     uint32_t scratch_base =
         (uint32_t)&REGION_NAME(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Base);
     uint32_t scratch_limit =
         (uint32_t)&REGION_NAME(Image$$, TFM_UNPRIV_SCRATCH, $$ZI$$Limit);
+#endif /* !defined(TFM_PSA_API) */
 
     /* Use the TT instruction to check access to the partition's regions*/
     range_access_allowed_by_mpu =
                           cmse_check_address_range((void *)p, s, flags) != NULL;
 
     if (range_access_allowed_by_mpu) {
-        return 1;
+        return TFM_SUCCESS;
     }
 
+#ifndef TFM_PSA_API /* Only use scratch if using veneer functions, not IPC */
     /* If the check for the current MPU settings fails, check for the share
      * region, only if the partition is secure
      */
     if ((flags & CMSE_NONSECURE) == 0) {
         if (check_address_range(p, s, scratch_base,
-                                scratch_limit+1-scratch_base)) {
-            return 1;
+                                scratch_limit+1-scratch_base) == TFM_SUCCESS) {
+            return TFM_SUCCESS;
         }
     }
+#endif /* !defined(TFM_PSA_API) */
 
     /* If all else fails, check whether the region is in the non-secure
      * memory
      */
-    return
-      check_address_range(p, s, NS_CODE_START, NS_CODE_LIMIT+1-NS_CODE_START) ||
-      check_address_range(p, s, NS_DATA_START, NS_DATA_LIMIT+1-NS_DATA_START);
+    if (check_address_range(p, s, NS_CODE_START,
+                            NS_CODE_LIMIT+1-NS_CODE_START) == TFM_SUCCESS ||
+        check_address_range(p, s, NS_DATA_START,
+                            NS_DATA_LIMIT+1-NS_DATA_START) == TFM_SUCCESS) {
+        return TFM_SUCCESS;
+    } else {
+        return TFM_ERROR_GENERIC;
+    }
 }
 
 int32_t tfm_core_has_read_access_to_region(const void *p, size_t s,
-                                           uint32_t ns_caller)
+                                           uint32_t ns_caller,
+                                           uint32_t privileged)
 {
-    int flags = CMSE_MPU_UNPRIV|CMSE_MPU_READ;
+    int flags = CMSE_MPU_READ;
+
+    if (privileged == TFM_PARTITION_UNPRIVILEGED_MODE) {
+        flags |= CMSE_MPU_UNPRIV;
+    }
 
     if (ns_caller) {
         flags |= CMSE_NONSECURE;
@@ -205,9 +150,14 @@ int32_t tfm_core_has_read_access_to_region(const void *p, size_t s,
 }
 
 int32_t tfm_core_has_write_access_to_region(void *p, size_t s,
-                                            uint32_t ns_caller)
+                                            uint32_t ns_caller,
+                                            uint32_t privileged)
 {
-    int flags = CMSE_MPU_UNPRIV|CMSE_MPU_READWRITE;
+    int flags = CMSE_MPU_READWRITE;
+
+    if (privileged == TFM_PARTITION_UNPRIVILEGED_MODE) {
+        flags |= CMSE_MPU_UNPRIV;
+    }
 
     if (ns_caller) {
         flags |= CMSE_NONSECURE;
@@ -216,453 +166,6 @@ int32_t tfm_core_has_write_access_to_region(void *p, size_t s,
     return has_access_to_region(p, s, flags);
 }
 
-/** \brief Check whether the iovec parameters are valid, and the memory ranges
- *         are in the posession of the calling partition
- *
- * \param[in] desc_ptr  The secure function request descriptor
- *
- * \return Return /ref TFM_SUCCESS if the iovec parameters are valid, error code
- *         otherwise as in /ref tfm_status_e
- */
-static int32_t tfm_core_check_sfn_parameters(
-                                           const struct tfm_sfn_req_s *desc_ptr)
-{
-    struct psa_invec *in_vec = (psa_invec *)desc_ptr->args[0];
-    size_t in_len;
-    struct psa_outvec *out_vec = (psa_outvec *)desc_ptr->args[2];
-    size_t out_len;
-
-    uint32_t i;
-
-    if ((desc_ptr->args[1] < 0) || (desc_ptr->args[3] < 0)) {
-        return TFM_ERROR_INVALID_PARAMETER;
-    }
-
-    in_len = (size_t)(desc_ptr->args[1]);
-    out_len = (size_t)(desc_ptr->args[3]);
-
-    /* The number of vectors are within range. Extra checks to avoid overflow */
-    if ((in_len > PSA_MAX_IOVEC) || (out_len > PSA_MAX_IOVEC) ||
-        (in_len + out_len > PSA_MAX_IOVEC)) {
-        return TFM_ERROR_INVALID_PARAMETER;
-    }
-
-    /* Check whether the caller partition has at write access to the iovec
-     * structures themselves. Use the TT instruction for this.
-     */
-    if (in_len > 0) {
-        if ((in_vec == NULL) ||
-            (tfm_core_has_write_access_to_region(in_vec,
-                                                 sizeof(psa_invec)*in_len,
-                                                 desc_ptr->ns_caller) != 1)) {
-            return TFM_ERROR_INVALID_PARAMETER;
-        }
-    } else {
-        if (in_vec != NULL) {
-            return TFM_ERROR_INVALID_PARAMETER;
-        }
-    }
-    if (out_len > 0) {
-        if ((out_vec == NULL) ||
-            (tfm_core_has_write_access_to_region(out_vec,
-                                                 sizeof(psa_outvec)*out_len,
-                                                 desc_ptr->ns_caller) != 1)) {
-            return TFM_ERROR_INVALID_PARAMETER;
-        }
-    } else {
-        if (out_vec != NULL) {
-            return TFM_ERROR_INVALID_PARAMETER;
-        }
-    }
-
-    /* Check whether the caller partition has access to the data inside the
-     * iovecs
-     */
-    for (i = 0; i < in_len; ++i) {
-        if (in_vec[i].len > 0) {
-            if ((in_vec[i].base == NULL) ||
-                (tfm_core_has_read_access_to_region(in_vec[i].base,
-                                                  in_vec[i].len,
-                                                  desc_ptr->ns_caller) != 1)) {
-                return TFM_ERROR_INVALID_PARAMETER;
-            }
-        }
-    }
-    for (i = 0; i < out_len; ++i) {
-        if (out_vec[i].len > 0) {
-            if ((out_vec[i].base == NULL) ||
-                (tfm_core_has_write_access_to_region(out_vec[i].base,
-                                                  out_vec[i].len,
-                                                  desc_ptr->ns_caller) != 1)) {
-                return TFM_ERROR_INVALID_PARAMETER;
-            }
-        }
-    }
-
-    return TFM_SUCCESS;
-}
-
-static void tfm_copy_iovec_parameters(struct iovec_args_t *target,
-                               const struct iovec_args_t *source)
-{
-    size_t i;
-
-    target->in_len = source->in_len;
-    for (i = 0; i < source->in_len; ++i) {
-        target->in_vec[i].base = source->in_vec[i].base;
-        target->in_vec[i].len = source->in_vec[i].len;
-    }
-    target->out_len = source->out_len;
-    for (i = 0; i < source->out_len; ++i) {
-        target->out_vec[i].base = source->out_vec[i].base;
-        target->out_vec[i].len = source->out_vec[i].len;
-    }
-}
-
-static void tfm_clear_iovec_parameters(struct iovec_args_t *args)
-{
-    int i;
-
-    args->in_len = 0;
-    for (i = 0; i < PSA_MAX_IOVEC; ++i) {
-        args->in_vec[i].base = NULL;
-        args->in_vec[i].len = 0;
-    }
-    args->out_len = 0;
-    for (i = 0; i < PSA_MAX_IOVEC; ++i) {
-        args->out_vec[i].base = NULL;
-        args->out_vec[i].len = 0;
-    }
-}
-
-static int32_t tfm_start_partition(const struct tfm_sfn_req_s *desc_ptr,
-                                                             uint32_t excReturn)
-{
-    uint32_t caller_partition_idx = desc_ptr->caller_part_idx;
-    const struct spm_partition_runtime_data_t *curr_part_data;
-    uint32_t caller_flags;
-    register uint32_t partition_idx;
-    uint32_t psp = __get_PSP();
-    uint32_t partition_psp, partition_psplim;
-    uint32_t partition_state;
-    uint32_t partition_flags;
-    struct tfm_exc_stack_t *svc_ctx = (struct tfm_exc_stack_t *)psp;
-    uint32_t caller_partition_id;
-    int32_t client_id;
-    struct iovec_args_t *iovec_args;
-
-    caller_flags = tfm_spm_partition_get_flags(caller_partition_idx);
-
-    /* Check partition state consistency */
-    if (((caller_flags & SPM_PART_FLAG_APP_ROT) != 0)
-        != (!desc_ptr->ns_caller)) {
-        /* Partition state inconsistency detected */
-        return TFM_SECURE_LOCK_FAILED;
-    }
-
-    if((caller_flags & SPM_PART_FLAG_APP_ROT) == 0) {
-        /* Disable NS exception handling while secure service is running.
-         * FixMe:
-         * This restriction is applied to limit the number of possible attack
-         * vectors.
-         * To be removed when pre-emption and context management issues have
-         * been analysed and resolved.
-         */
-        TFM_NS_EXC_DISABLE();
-    }
-
-    partition_idx = get_partition_idx(desc_ptr->sp_id);
-
-    curr_part_data = tfm_spm_partition_get_runtime_data(partition_idx);
-    partition_state = curr_part_data->partition_state;
-    partition_flags = tfm_spm_partition_get_flags(partition_idx);
-    caller_partition_id = tfm_spm_partition_get_partition_id(
-                                                          caller_partition_idx);
-
-    if (tfm_secure_api_initializing) {
-#if TFM_LVL != 1
-        /* Make thread mode unprivileged while untrusted partition init is
-         * executed
-         */
-        if ((partition_flags & SPM_PART_FLAG_PSA_ROT) == 0) {
-            CONTROL_Type ctrl;
-
-            ctrl.w = __get_CONTROL();
-            ctrl.b.nPRIV = 1;
-            __set_CONTROL(ctrl.w);
-            __DSB();
-            __ISB();
-        }
-#endif
-    } else if (partition_state == SPM_PARTITION_STATE_RUNNING ||
-        partition_state == SPM_PARTITION_STATE_SUSPENDED ||
-        partition_state == SPM_PARTITION_STATE_BLOCKED) {
-        /* Recursion is not permitted! */
-        return TFM_ERROR_PARTITION_NON_REENTRANT;
-    } else if (partition_state != SPM_PARTITION_STATE_IDLE) {
-        /* The partition to be called is not in a proper state */
-        return TFM_SECURE_LOCK_FAILED;
-    }
-
-#if TFM_LVL == 1
-    /* Prepare switch to shared secure partition stack */
-    /* In case the call is coming from the non-secure world, we save the iovecs
-     * on the stop of the stack. So the memory area, that can actually be used
-     * as stack by the partitions starts at a lower address
-     */
-    partition_psp =
-        (uint32_t)&REGION_NAME(Image$$, TFM_SECURE_STACK, $$ZI$$Limit)-
-        sizeof(struct iovec_args_t);
-    partition_psplim =
-        (uint32_t)&REGION_NAME(Image$$, TFM_SECURE_STACK, $$ZI$$Base);
-#else
-    partition_psp = curr_part_data->stack_ptr;
-    partition_psplim = tfm_spm_partition_get_stack_bottom(partition_idx);
-#endif
-    /* Store the context for the partition call */
-    tfm_spm_partition_set_caller_partition_idx(partition_idx,
-                                               caller_partition_idx);
-    tfm_spm_partition_store_context(caller_partition_idx, psp, excReturn);
-
-    if ((caller_flags & SPM_PART_FLAG_APP_ROT)) {
-        tfm_spm_partition_set_caller_client_id(partition_idx,
-                                               caller_partition_id);
-    } else {
-        client_id = tfm_nspm_get_current_client_id();
-        if (client_id >= 0)
-        {
-            return TFM_SECURE_LOCK_FAILED;
-        }
-        tfm_spm_partition_set_caller_client_id(partition_idx, client_id);
-    }
-
-#if (TFM_LVL != 1) && (TFM_LVL != 2)
-    /* Dynamic partitioning is only done is TFM level 3 */
-    tfm_spm_partition_sandbox_deconfig(caller_partition_idx);
-
-    /* Configure partition execution environment */
-    if (tfm_spm_partition_sandbox_config(partition_idx) != SPM_ERR_OK) {
-        ERROR_MSG("Failed to configure sandbox for partition!");
-        tfm_secure_api_error_handler();
-    }
-#endif
-
-    /* Default share to scratch area in case of partition to partition calls
-     * this way partitions always get default access to input buffers
-     */
-    /* FixMe: return value/error handling TBD */
-    tfm_spm_partition_set_share(partition_idx, desc_ptr->ns_caller ?
-        TFM_BUFFER_SHARE_NS_CODE : TFM_BUFFER_SHARE_SCRATCH);
-
-#if TFM_LVL == 1
-    /* In level one, only switch context and return from exception if in
-     * handler mode
-     */
-    if ((desc_ptr->ns_caller) || (tfm_secure_api_initializing)) {
-        if (desc_ptr->iovec_api == TFM_SFN_API_IOVEC) {
-            /* Save the iovecs on the common stack. The vectors had been sanity
-             * checked already, and since then the interrupts have been kept
-             * disabled. So we can be sure that the vectors haven't been
-             * tampered with since the check.
-             */
-            iovec_args = (struct iovec_args_t *)
-                    ((uint32_t)&REGION_NAME(Image$$, TFM_SECURE_STACK, $$ZI$$Limit)-
-                     sizeof(struct iovec_args_t));
-            if (tfm_spm_partition_set_iovec(partition_idx, desc_ptr->args) !=
-                SPM_ERR_OK) {
-                return TFM_ERROR_GENERIC;
-            }
-            tfm_copy_iovec_parameters(iovec_args,
-                                      &(curr_part_data->iovec_args));
-
-            /* Prepare the partition context, update stack ptr */
-            psp = (uint32_t)prepare_partition_iovec_ctx(svc_ctx, desc_ptr,
-                                                        iovec_args,
-                                                     (uint32_t *)partition_psp);
-        } else {
-            /* Prepare the partition context, update stack ptr */
-            psp = (uint32_t)prepare_partition_ctx(svc_ctx, desc_ptr,
-                                                  (uint32_t *)partition_psp);
-        }
-        __set_PSP(psp);
-        __set_PSPLIM(partition_psplim);
-    }
-#else
-    if (desc_ptr->iovec_api == TFM_SFN_API_IOVEC) {
-        /* Save the iovecs on the stack of the partition. The vectors had been
-         * sanity checked already, and since then the interrupts have been kept
-         * disabled. So we can be sure that the vectors haven't been tampered
-         * with since the check.
-         */
-        iovec_args =
-        (struct iovec_args_t *)(tfm_spm_partition_get_stack_top(partition_idx) -
-        sizeof(struct iovec_args_t));
-        if (tfm_spm_partition_set_iovec(partition_idx, desc_ptr->args) !=
-            SPM_ERR_OK) {
-            return TFM_ERROR_GENERIC;
-        }
-        tfm_copy_iovec_parameters(iovec_args, &(curr_part_data->iovec_args));
-
-        /* Prepare the partition context, update stack ptr */
-        psp = (uint32_t)prepare_partition_iovec_ctx(svc_ctx, desc_ptr,
-                                                    iovec_args,
-                                                    (uint32_t *)partition_psp);
-    } else {
-        /* Prepare the partition context, update stack ptr */
-        psp = (uint32_t)prepare_partition_ctx(svc_ctx, desc_ptr,
-                                              (uint32_t *)partition_psp);
-    }
-    __set_PSP(psp);
-    __set_PSPLIM(partition_psplim);
-#endif
-
-    tfm_spm_partition_set_state(caller_partition_idx,
-                                SPM_PARTITION_STATE_BLOCKED);
-    tfm_spm_partition_set_state(partition_idx, SPM_PARTITION_STATE_RUNNING);
-    tfm_secure_lock++;
-
-    return TFM_SUCCESS;
-}
-
-static int32_t tfm_return_from_partition(uint32_t *excReturn)
-{
-    uint32_t current_partition_idx =
-            tfm_spm_partition_get_running_partition_idx();
-    const struct spm_partition_runtime_data_t *curr_part_data, *ret_part_data;
-    uint32_t current_partition_flags;
-    uint32_t return_partition_idx;
-    uint32_t return_partition_flags;
-    uint32_t psp = __get_PSP();
-    size_t i;
-    struct tfm_exc_stack_t *svc_ctx = (struct tfm_exc_stack_t *)psp;
-    struct iovec_args_t *iovec_args;
-
-    if (current_partition_idx == SPM_INVALID_PARTITION_IDX) {
-        return TFM_SECURE_UNLOCK_FAILED;
-    }
-
-    curr_part_data = tfm_spm_partition_get_runtime_data(current_partition_idx);
-    return_partition_idx = curr_part_data->caller_partition_idx;
-
-    if (return_partition_idx == SPM_INVALID_PARTITION_IDX) {
-        return TFM_SECURE_UNLOCK_FAILED;
-    }
-
-    ret_part_data = tfm_spm_partition_get_runtime_data(return_partition_idx);
-
-    return_partition_flags = tfm_spm_partition_get_flags(return_partition_idx);
-    current_partition_flags = tfm_spm_partition_get_flags(
-            current_partition_idx);
-
-    tfm_secure_lock--;
-
-    if((return_partition_flags & SPM_PART_FLAG_APP_ROT) == 0) {
-        /* Re-enable NS exceptions when secure service returns to NS client.
-         * FixMe:
-         * To be removed when pre-emption and context management issues have
-         * been analysed and resolved.
-         */
-        TFM_NS_EXC_ENABLE();
-    }
-
-#if (TFM_LVL != 1) && (TFM_LVL != 2)
-    /* Deconfigure completed partition environment */
-    tfm_spm_partition_sandbox_deconfig(current_partition_idx);
-    if (tfm_secure_api_initializing) {
-        /* Restore privilege for thread mode during TF-M init. This is only
-         * have to be done if the partition is not trusted.
-         */
-        if ((current_partition_flags & SPM_PART_FLAG_PSA_ROT) == 0) {
-            CONTROL_Type ctrl;
-
-            ctrl.w = __get_CONTROL();
-            ctrl.b.nPRIV = 0;
-            __set_CONTROL(ctrl.w);
-            __DSB();
-            __ISB();
-        }
-    } else {
-        /* Configure the caller partition environment in case this was a
-         * partition to partition call and returning to untrusted partition
-         */
-        if (tfm_spm_partition_sandbox_config(return_partition_idx)
-            != SPM_ERR_OK) {
-            ERROR_MSG("Failed to configure sandbox for partition!");
-            tfm_secure_api_error_handler();
-        }
-        if (return_partition_flags & SPM_PART_FLAG_APP_ROT) {
-            /* Restore share status */
-            tfm_spm_partition_set_share(
-                return_partition_idx,
-                tfm_spm_partition_get_runtime_data(
-                    return_partition_idx)->share);
-        }
-    }
-#endif
-
-#if TFM_LVL == 1
-    if (!(return_partition_flags & SPM_PART_FLAG_APP_ROT) ||
-        (tfm_secure_api_initializing)) {
-        /* In TFM level 1 context restore is only done when
-         * returning to NS or after initialization
-         */
-        /* Restore caller context */
-        restore_caller_ctx(svc_ctx,
-            (struct tfm_exc_stack_t *)ret_part_data->stack_ptr);
-        *excReturn = ret_part_data->lr;
-        __set_PSP(ret_part_data->stack_ptr);
-        extern uint32_t Image$$ARM_LIB_STACK$$ZI$$Base[];
-        uint32_t psp_stack_bottom = (uint32_t)Image$$ARM_LIB_STACK$$ZI$$Base;
-       __set_PSPLIM(psp_stack_bottom);
-
-        /* FIXME: The condition should be removed once all the secure service
-         *        calls are done via the iovec veneers */
-        if (curr_part_data->iovec_api) {
-            iovec_args = (struct iovec_args_t *)
-                         ((uint32_t)&REGION_NAME(Image$$, TFM_SECURE_STACK, $$ZI$$Limit)-
-                         sizeof(struct iovec_args_t));
-
-            for (i = 0; i < curr_part_data->iovec_args.out_len; ++i) {
-                curr_part_data->orig_outvec[i].len = iovec_args->out_vec[i].len;
-            }
-            tfm_clear_iovec_parameters(iovec_args);
-        }
-    }
-#else
-    /* Restore caller context */
-    restore_caller_ctx(svc_ctx,
-        (struct tfm_exc_stack_t *)ret_part_data->stack_ptr);
-    *excReturn = ret_part_data->lr;
-    __set_PSP(ret_part_data->stack_ptr);
-    __set_PSPLIM(tfm_spm_partition_get_stack_bottom(return_partition_idx));
-    /* Clear the context entry before returning */
-    tfm_spm_partition_set_stack(
-                current_partition_idx, psp + sizeof(struct tfm_exc_stack_t));
-
-    /* FIXME: The condition should be removed once all the secure service
-     *        calls are done via the iovec veneers */
-    if (curr_part_data->iovec_api) {
-        iovec_args = (struct iovec_args_t *)
-                     (tfm_spm_partition_get_stack_top(current_partition_idx) -
-                     sizeof(struct iovec_args_t));
-
-        for (i = 0; i < curr_part_data->iovec_args.out_len; ++i) {
-            curr_part_data->orig_outvec[i].len = iovec_args->out_vec[i].len;
-        }
-        tfm_clear_iovec_parameters(iovec_args);
-    }
-#endif
-
-    tfm_spm_partition_cleanup_context(current_partition_idx);
-
-    tfm_spm_partition_set_state(current_partition_idx,
-                                SPM_PARTITION_STATE_IDLE);
-    tfm_spm_partition_set_state(return_partition_idx,
-                                SPM_PARTITION_STATE_RUNNING);
-
-    return TFM_SUCCESS;
-}
-
 void tfm_secure_api_error_handler(void)
 {
     ERROR_MSG("Security violation when calling secure API");
@@ -671,487 +174,3 @@ void tfm_secure_api_error_handler(void)
     }
 }
 
-static int32_t tfm_check_sfn_req_integrity(const struct tfm_sfn_req_s *desc_ptr)
-{
-    if ((desc_ptr == NULL) ||
-        (desc_ptr->sp_id == 0) ||
-        (desc_ptr->sfn == NULL)) {
-        /* invalid parameter */
-        return TFM_ERROR_INVALID_PARAMETER;
-    }
-    return TFM_SUCCESS;
-}
-
-static int32_t tfm_core_check_sfn_req_rules(
-        const struct tfm_sfn_req_s *desc_ptr)
-{
-    /* Check partition idx validity */
-    if (desc_ptr->caller_part_idx == SPM_INVALID_PARTITION_IDX) {
-        return TFM_ERROR_NO_ACTIVE_PARTITION;
-    }
-
-    if ((desc_ptr->ns_caller) && (tfm_secure_lock != 0)) {
-        /* Secure domain is already locked!
-         * This should only happen if caller is secure partition!
-         * FixMe: This scenario is a potential security breach
-         * Take appropriate action!
-         */
-        return TFM_ERROR_SECURE_DOMAIN_LOCKED;
-    }
-
-    if (tfm_secure_api_initializing) {
-        int32_t id =
-            tfm_spm_partition_get_partition_id(desc_ptr->caller_part_idx);
-
-        if ((id != TFM_SP_CORE_ID) || (tfm_secure_lock != 0)) {
-            /* Invalid request during system initialization */
-            ERROR_MSG("Invalid service request during initialization!");
-            return TFM_ERROR_NOT_INITIALIZED;
-        }
-    }
-
-    return TFM_SUCCESS;
-}
-
-void tfm_secure_api_init_done(void)
-{
-    tfm_secure_api_initializing = 0;
-#if TFM_LVL != 1
-    if (tfm_spm_partition_sandbox_config(TFM_SP_NON_SECURE_ID) != SPM_ERR_OK) {
-        ERROR_MSG("Failed to configure sandbox for partition!");
-        tfm_secure_api_error_handler();
-    }
-#endif
-}
-
-int32_t tfm_core_sfn_request_handler(
-                             struct tfm_sfn_req_s *desc_ptr, uint32_t excReturn)
-{
-    int32_t res;
-
-    res = tfm_check_sfn_req_integrity(desc_ptr);
-    if (res != TFM_SUCCESS) {
-        ERROR_MSG("Invalid service request!");
-        tfm_secure_api_error_handler();
-    }
-
-    __disable_irq();
-
-    desc_ptr->caller_part_idx = tfm_spm_partition_get_running_partition_idx();
-
-    if (desc_ptr->iovec_api == TFM_SFN_API_IOVEC) {
-        res = tfm_core_check_sfn_parameters(desc_ptr);
-        if (res != TFM_SUCCESS) {
-            /* The sanity check of iovecs failed. */
-            __enable_irq();
-            tfm_secure_api_error_handler();
-        }
-    }
-
-    res = tfm_core_check_sfn_req_rules(desc_ptr);
-    if (res != TFM_SUCCESS) {
-        /* FixMe: error compartmentalization TBD */
-        tfm_spm_partition_set_state(
-            desc_ptr->caller_part_idx, SPM_PARTITION_STATE_CLOSED);
-        __enable_irq();
-        ERROR_MSG("Unauthorized service request!");
-        tfm_secure_api_error_handler();
-    }
-
-    res = tfm_start_partition(desc_ptr, excReturn);
-    if (res != TFM_SUCCESS) {
-        /* FixMe: consider possible fault scenarios */
-        __enable_irq();
-        ERROR_MSG("Failed to process service request!");
-        tfm_secure_api_error_handler();
-    }
-
-    __enable_irq();
-
-    return res;
-}
-
-#if TFM_LVL == 1
-int32_t tfm_core_sfn_request_thread_mode(struct tfm_sfn_req_s *desc_ptr)
-{
-    int32_t res;
-    int32_t *args;
-    int32_t retVal;
-
-    if (desc_ptr->iovec_api == TFM_SFN_API_IOVEC) {
-        res = tfm_core_check_sfn_parameters(desc_ptr);
-        if (res != TFM_SUCCESS) {
-            /* The sanity check of iovecs failed. */
-            return res;
-        }
-    }
-
-    /* No excReturn value is needed as no exception handling is used */
-    res = tfm_core_sfn_request_handler(desc_ptr, 0);
-
-    if (res != TFM_SUCCESS) {
-        tfm_secure_api_error_handler();
-    }
-
-    /* Secure partition to secure partition call in TFM level 1 */
-    args = desc_ptr->args;
-    retVal = desc_ptr->sfn(args[0], args[1], args[2], args[3]);
-
-    /* return handler should restore original exc_return value... */
-    res = tfm_return_from_partition(NULL);
-    if (res == TFM_SUCCESS) {
-        /* If unlock successful, pass SS return value to caller */
-        res = retVal;
-    } else {
-        /* Unlock errors indicate ctx database corruption or unknown
-         * anomalies. Halt execution
-         */
-        ERROR_MSG("Secure API error during unlock!");
-        tfm_secure_api_error_handler();
-    }
-    return res;
-}
-#endif
-
-void tfm_core_validate_secure_caller_handler(uint32_t *svc_args)
-{
-
-    int32_t res = TFM_ERROR_GENERIC;
-    uint32_t running_partition_idx =
-            tfm_spm_partition_get_running_partition_idx();
-    const struct spm_partition_runtime_data_t *curr_part_data =
-            tfm_spm_partition_get_runtime_data(running_partition_idx);
-    uint32_t running_partition_flags =
-            tfm_spm_partition_get_flags(running_partition_idx);
-    uint32_t caller_partition_flags =
-            tfm_spm_partition_get_flags(curr_part_data->caller_partition_idx);
-
-    if (!(running_partition_flags & SPM_PART_FLAG_APP_ROT))  {
-        /* This handler shouldn't be called from outside partition context.
-         * Partitions are only allowed to run while S domain is locked.
-         */
-        svc_args[0] = TFM_ERROR_INVALID_PARAMETER;
-        return;
-    }
-
-    /* Store return value in r0 */
-    if (caller_partition_flags & SPM_PART_FLAG_APP_ROT) {
-        res = TFM_SUCCESS;
-    }
-    svc_args[0] = res;
-}
-
-int32_t tfm_core_check_buffer_access(uint32_t  partition_idx,
-                                     void     *start_addr,
-                                     size_t    len,
-                                     uint32_t  alignment)
-{
-    uintptr_t start_addr_value = (uintptr_t)start_addr;
-    uintptr_t end_addr_value = (uintptr_t)start_addr + len;
-    uintptr_t alignment_mask;
-
-    alignment_mask = (((uintptr_t)1) << alignment) - 1;
-
-    /* Check that the pointer is aligned properly */
-    if (start_addr_value & alignment_mask) {
-        /* not aligned, return error */
-        return 0;
-    }
-
-    /* Protect against overflow (and zero len) */
-    if (end_addr_value <= start_addr_value)
-    {
-        return 0;
-    }
-
-#if TFM_LVL == 1
-    /* For privileged partition execution, all secure data memory and stack
-     * is accessible
-     */
-    if (start_addr_value >= S_DATA_START &&
-        end_addr_value <= (S_DATA_START + S_DATA_SIZE)) {
-        return 1;
-    }
-#else
-    /* For non-privileged execution the partition's data and stack is
-     * accessible
-     */
-    if (start_addr_value >=
-            tfm_spm_partition_get_stack_bottom(partition_idx) &&
-        end_addr_value <=
-            tfm_spm_partition_get_stack_top(partition_idx)) {
-        return 1;
-    }
-    if (start_addr_value >=
-           tfm_spm_partition_get_rw_start(partition_idx) &&
-        end_addr_value <=
-           tfm_spm_partition_get_rw_limit(partition_idx)) {
-        return 1;
-    }
-    if (start_addr_value >=
-           tfm_spm_partition_get_zi_start(partition_idx) &&
-        end_addr_value <=
-           tfm_spm_partition_get_zi_limit(partition_idx)) {
-        return 1;
-    }
-#endif
-    return 0;
-}
-
-void tfm_core_get_caller_client_id_handler(uint32_t *svc_args)
-{
-    uintptr_t result_ptr_value = svc_args[0];
-    uint32_t running_partition_idx =
-            tfm_spm_partition_get_running_partition_idx();
-    const uint32_t running_partition_flags =
-            tfm_spm_partition_get_flags(running_partition_idx);
-    const struct spm_partition_runtime_data_t *curr_part_data =
-            tfm_spm_partition_get_runtime_data(running_partition_idx);
-    int res = 0;
-
-    if (!(running_partition_flags & SPM_PART_FLAG_APP_ROT))  {
-        /* This handler shouldn't be called from outside partition context.
-         * Partitions are only allowed to run while S domain is locked.
-         */
-        svc_args[0] = TFM_ERROR_INVALID_PARAMETER;
-        return;
-    }
-
-    /* Make sure that the output pointer points to a memory area that is owned
-     * by the partition
-     */
-    res = tfm_core_check_buffer_access(running_partition_idx,
-                                       (void *)result_ptr_value,
-                                       sizeof(curr_part_data->caller_client_id),
-                                       2);
-    if (!res) {
-        /* Not in accessible range, return error */
-        svc_args[0] = TFM_ERROR_INVALID_PARAMETER;
-        return;
-    }
-
-    *((int32_t *)result_ptr_value) = curr_part_data->caller_client_id;
-
-    /* Store return value in r0 */
-    svc_args[0] = TFM_SUCCESS;
-}
-
-void tfm_core_memory_permission_check_handler(uint32_t *svc_args)
-{
-    uint32_t ptr = svc_args[0];
-    uint32_t size = svc_args[1];
-    int32_t access = svc_args[2];
-
-    uint32_t max_buf_size, ptr_start, range_limit, range_check = false;
-    int32_t res;
-    uint32_t running_partition_idx =
-            tfm_spm_partition_get_running_partition_idx();
-    const struct spm_partition_runtime_data_t *curr_part_data =
-            tfm_spm_partition_get_runtime_data(running_partition_idx);
-    uint32_t running_partition_flags =
-            tfm_spm_partition_get_flags(running_partition_idx);
-    int32_t flags = 0;
-    void *rangeptr;
-
-    if (!(running_partition_flags & SPM_PART_FLAG_APP_ROT) || (size == 0)) {
-        /* This handler should only be called from a secure partition. */
-        svc_args[0] = TFM_ERROR_INVALID_PARAMETER;
-        return;
-    }
-
-    if (curr_part_data->share != TFM_BUFFER_SHARE_PRIV) {
-        flags |= CMSE_MPU_UNPRIV;
-    }
-
-    if (access == TFM_MEMORY_ACCESS_RW) {
-        flags |= CMSE_MPU_READWRITE;
-    } else {
-        flags |= CMSE_MPU_READ;
-    }
-
-    /* Check if partition access to address would fail */
-    rangeptr = cmse_check_address_range((void *)ptr, size, flags);
-
-    /* Get regions associated with address */
-    cmse_address_info_t addr_info = cmse_TT((void *)ptr);
-
-    if (rangeptr == NULL) {
-        svc_args[0] = TFM_ERROR_INVALID_PARAMETER;
-        return;
-    }
-
-    if (addr_info.flags.secure) {
-#if TFM_LVL == 1
-        /* For privileged partition execution, all secure data memory is
-         * accessible
-         */
-        max_buf_size = S_DATA_SIZE;
-        ptr_start = S_DATA_START;
-        range_limit = S_DATA_LIMIT;
-#else
-        /* Only scratch is permitted in secure memory */
-        max_buf_size = (uint32_t)tfm_scratch_area_size;
-        ptr_start = (uint32_t)tfm_scratch_area;
-        range_limit = (uint32_t)tfm_scratch_area + tfm_scratch_area_size - 1;
-#endif
-        range_check = true;
-    } else {
-        if (!addr_info.flags.sau_region_valid) {
-            /* If address is NS, TF-M expects SAU to be configured
-             */
-            svc_args[0] = TFM_ERROR_INVALID_PARAMETER;
-            return;
-        }
-        switch (addr_info.flags.sau_region) {
-        case TFM_NS_REGION_CODE:
-            if (access == TFM_MEMORY_ACCESS_RW) {
-                res = TFM_ERROR_INVALID_PARAMETER;
-            } else {
-                /* Currently TF-M does not support checks for NS Memory
-                 * accesses by partitions
-                 */
-                res = TFM_SUCCESS;
-            }
-            break;
-        case TFM_NS_REGION_DATA:
-            /* Currently TF-M does not support checks for NS Memory
-             * accesses by partitions
-             */
-            res = TFM_SUCCESS;
-            break;
-        default:
-            /* Only NS data and code regions can be accessed as buffers */
-            res = TFM_ERROR_INVALID_PARAMETER;
-            break;
-        }
-    }
-
-    if (range_check == true) {
-        if ((size <= max_buf_size) && (ptr >= ptr_start)
-            && (ptr <= range_limit + 1 - size)) {
-            res = TFM_SUCCESS;
-        } else {
-            res = TFM_ERROR_INVALID_PARAMETER;
-        }
-    }
-
-    /* Store return value in r0 */
-    svc_args[0] = res;
-}
-
-/* This SVC handler is called if veneer is running in thread mode */
-uint32_t tfm_core_partition_request_svc_handler(
-        const struct tfm_exc_stack_t *svc_ctx, uint32_t excReturn)
-{
-    struct tfm_sfn_req_s *desc_ptr;
-
-    if (!(excReturn & EXC_RETURN_STACK_PROCESS)) {
-        /* Service request SVC called with MSP active.
-         * Either invalid configuration for Thread mode or SVC called
-         * from Handler mode, which is not supported.
-         * FixMe: error severity TBD
-         */
-        ERROR_MSG("Service request SVC called with MSP active!");
-        tfm_secure_api_error_handler();
-    }
-
-    desc_ptr = (struct tfm_sfn_req_s *)svc_ctx->R0;
-
-    if (tfm_core_sfn_request_handler(desc_ptr, excReturn) != TFM_SUCCESS) {
-        tfm_secure_api_error_handler();
-    }
-
-    return EXC_RETURN_SECURE_FUNCTION;
-}
-
-/* This SVC handler is called when sfn returns */
-uint32_t tfm_core_partition_return_handler(uint32_t lr)
-{
-    int32_t res;
-
-    if (!(lr & EXC_RETURN_STACK_PROCESS)) {
-        /* Partition return SVC called with MSP active.
-         * This should not happen!
-         */
-        ERROR_MSG("Partition return SVC called with MSP active!");
-        tfm_secure_api_error_handler();
-    }
-
-    /* Store return value from secure partition */
-    int32_t retVal = *(int32_t *)__get_PSP();
-
-    if (!is_iovec_api_call()) {
-        if ((retVal > TFM_SUCCESS) &&
-            (retVal < TFM_PARTITION_SPECIFIC_ERROR_MIN)) {
-            /* Secure function returned a reserved value */
-#ifdef TFM_CORE_DEBUG
-            LOG_MSG("Invalid return value from secure partition!");
-#endif
-            /* FixMe: error can be traced to specific secure partition
-             * and Core is not compromised. Error handling flow can be
-             * refined
-             */
-            tfm_secure_api_error_handler();
-        }
-    }
-
-    res = tfm_return_from_partition(&lr);
-    if (res != TFM_SUCCESS) {
-        /* Unlock errors indicate ctx database corruption or unknown anomalies
-         * Halt execution
-         */
-        ERROR_MSG("Secure API error during unlock!");
-        tfm_secure_api_error_handler();
-    }
-
-    return lr;
-}
-
-void tfm_core_set_buffer_area_handler(uint32_t *args)
-{
-    /* r0 is stored in args[0] in exception stack frame
-     * Store input parameter before writing return value to that address
-     */
-    enum tfm_buffer_share_region_e share;
-    uint32_t running_partition_idx =
-            tfm_spm_partition_get_running_partition_idx();
-    const struct spm_partition_runtime_data_t *curr_part_data =
-            tfm_spm_partition_get_runtime_data(running_partition_idx);
-    uint32_t caller_partition_idx = curr_part_data->caller_partition_idx;
-    uint32_t running_partition_flags =
-            tfm_spm_partition_get_flags(running_partition_idx);
-    uint32_t caller_partition_flags =
-            tfm_spm_partition_get_flags(caller_partition_idx);
-
-     /* tfm_core_set_buffer_area() returns int32_t */
-    int32_t *res_ptr = (int32_t *)&args[0];
-
-    if (!(running_partition_flags & SPM_PART_FLAG_APP_ROT)) {
-        /* This handler should only be called from a secure partition. */
-        *res_ptr = TFM_ERROR_INVALID_PARAMETER;
-        return;
-    }
-
-    switch (args[0]) {
-    case TFM_BUFFER_SHARE_DEFAULT:
-        share = (!(caller_partition_flags & SPM_PART_FLAG_APP_ROT)) ?
-            (TFM_BUFFER_SHARE_NS_CODE) : (TFM_BUFFER_SHARE_SCRATCH);
-        break;
-    case TFM_BUFFER_SHARE_SCRATCH:
-    case TFM_BUFFER_SHARE_NS_CODE:
-        share = args[0];
-        break;
-    default:
-        *res_ptr = TFM_ERROR_INVALID_PARAMETER;
-        return;
-    }
-
-    if (tfm_spm_partition_set_share(running_partition_idx, share) ==
-            SPM_ERR_OK) {
-        *res_ptr = TFM_SUCCESS;
-    } else {
-        *res_ptr = TFM_ERROR_INVALID_PARAMETER;
-    }
-
-    return;
-}
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.h
index 50f635708e..02f766a8e6 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_secure_api.h
@@ -96,14 +96,19 @@ int32_t tfm_core_sfn_request_thread_mode(struct tfm_sfn_req_s *desc_ptr);
  * This function assumes, that the current MPU configuration is set for the
  * partition to be checked.
  *
- * \param[in] p          The start address of the range to check
- * \param[in] s          The size of the range to check
- * \param[in] ns_caller  Whether the current partition is a non-secure one
+ * \param[in] p                The start address of the range to check
+ * \param[in] s                The size of the range to check
+ * \param[in] ns_caller        Whether the current partition is a non-secure one
+ * \param[in] privileged       Privileged mode or unprivileged mode:
+ *                             \ref TFM_PARTITION_UNPRIVILEGED_MODE
+ *                             \ref TFM_PARTITION_PRIVILEGED_MODE
  *
- * \return 1 if the partition has access to the memory range, 0 otherwise.
+ * \return TFM_SUCCESS if the partition has access to the memory range,
+ *         TFM_ERROR_GENERIC otherwise.
  */
 int32_t tfm_core_has_read_access_to_region(const void *p, size_t s,
-                                           uint32_t ns_caller);
+                                           uint32_t ns_caller,
+                                           uint32_t privileged);
 
 /**
  * \brief Check whether the current partition has write access to a memory range
@@ -111,15 +116,39 @@ int32_t tfm_core_has_read_access_to_region(const void *p, size_t s,
  * This function assumes, that the current MPU configuration is set for the
  * partition to be checked.
  *
- * \param[in] p          The start address of the range to check
- * \param[in] s          The size of the range to check
- * \param[in] ns_caller  Whether the current partition is a non-secure one
+ * \param[in] p                The start address of the range to check
+ * \param[in] s                The size of the range to check
+ * \param[in] ns_caller        Whether the current partition is a non-secure one
+ * \param[in] privileged       Privileged mode or unprivileged mode:
+ *                             \ref TFM_PARTITION_UNPRIVILEGED_MODE
+ *                             \ref TFM_PARTITION_PRIVILEGED_MODE
  *
- * \return 1 if the partition has access to the memory range, 0 otherwise.
+ * \return TFM_SUCCESS if the partition has access to the memory range,
+ *         TFM_ERROR_GENERIC otherwise.
  */
 int32_t tfm_core_has_write_access_to_region(void *p, size_t s,
-                                            uint32_t ns_caller);
+                                            uint32_t ns_caller,
+                                            uint32_t privileged);
 
+#ifdef TFM_PSA_API
+/* The following macros are only valid if secure services can be called
+ * using veneer functions. This is not the case if IPC messaging is enabled
+ */
+#define TFM_CORE_IOVEC_SFN_REQUEST(id, fn, a, b, c, d)               \
+        do {                                                         \
+            ERROR_MSG("Invalid TF-M configuration detected");        \
+            tfm_secure_api_error_handler();                          \
+            /* This point never reached */                           \
+            return (int32_t)TFM_ERROR_GENERIC;                       \
+        } while (0)
+#define TFM_CORE_SFN_REQUEST(id, fn, a, b, c, d)                     \
+        do {                                                         \
+            ERROR_MSG("Invalid TF-M configuration detected");        \
+            tfm_secure_api_error_handler();                          \
+            /* This point never reached */                           \
+            return (int32_t)TFM_ERROR_GENERIC;                       \
+        } while (0)
+#else
 #define TFM_CORE_IOVEC_SFN_REQUEST(id, fn, a, b, c, d) \
         return tfm_core_partition_request(id, fn, TFM_SFN_API_IOVEC, \
                 (int32_t)a, (int32_t)b, (int32_t)c, (int32_t)d)
@@ -136,7 +165,7 @@ int32_t tfm_core_partition_request(uint32_t id, void *fn, int32_t iovec_api,
     struct tfm_sfn_req_s desc, *desc_ptr = &desc;
 
     desc.sp_id = id;
-    desc.sfn = fn;
+    desc.sfn = (sfn_t) fn;
     desc.args = args;
     /*
      * This preprocessor condition checks if a version of GCC smaller than
@@ -180,5 +209,6 @@ int32_t tfm_core_partition_request(uint32_t id, void *fn, int32_t iovec_api,
 
     }
 }
+#endif
 
 #endif /* __TFM_SECURE_API_H__ */
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_spm_services.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_spm_services.c
index dadf66f02c..83ff41b674 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_spm_services.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_spm_services.c
@@ -11,6 +11,7 @@
 #include "tfm_secure_api.h"
 #include "tfm_internal.h"
 #include "secure_fw/include/tfm_spm_services_api.h"
+#include "spm_api.h"
 
 uint8_t *tfm_scratch_area;
 uint32_t tfm_scratch_area_size;
@@ -18,13 +19,9 @@ nsfptr_t ns_entry;
 
 void jump_to_ns_code(void)
 {
-#if TFM_LVL != 1
+#if TFM_LVL == 3 || ((!defined(TFM_PSA_API)) && (TFM_LVL != 1))
     /* Initialization is done, set thread mode to unprivileged. */
-    CONTROL_Type ctrl;
-
-    ctrl.w = __get_CONTROL();
-    ctrl.b.nPRIV = 1;
-    __set_CONTROL(ctrl.w);
+    tfm_spm_partition_change_privilege(TFM_PARTITION_UNPRIVILEGED_MODE);
 #endif
     /* All changes made to memory will be effective after this point */
     __DSB();
@@ -34,6 +31,7 @@ void jump_to_ns_code(void)
     ns_entry();
 }
 
+#ifndef TFM_PSA_API
 #if defined(__ARM_ARCH_8M_MAIN__)
 __attribute__((naked)) int32_t tfm_core_sfn_request(
                                            const struct tfm_sfn_req_s *desc_ptr)
@@ -154,6 +152,7 @@ int32_t tfm_core_set_buffer_area(enum tfm_buffer_share_region_e share)
         "BX     lr\n"
         : : "I" (TFM_SVC_SET_SHARE_AREA));
 }
+#endif
 
 __attribute__((naked))
 int32_t tfm_core_get_boot_data(uint8_t major_type,
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_svc.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_svc.h
index e4ed34b1b6..e0c11021d3 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_svc.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_svc.h
@@ -23,6 +23,7 @@ typedef enum {
 #ifdef TFM_PSA_API
     TFM_SVC_IPC_REQUEST,
     TFM_SVC_SCHEDULE,
+    TFM_SVC_EXIT_THRD,
     /* PSA Client SVC */
     TFM_SVC_PSA_FRAMEWORK_VERSION,
     TFM_SVC_PSA_VERSION,
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_psa_api_client.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/ns_callable/tfm_psa_api_veneers.c
similarity index 99%
rename from components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_psa_api_client.c
rename to components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/ns_callable/tfm_psa_api_veneers.c
index 3c341bd902..7d8ff20260 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/core/tfm_psa_api_client.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/ns_callable/tfm_psa_api_veneers.c
@@ -43,7 +43,7 @@ int32_t tfm_psa_veneer_sanity_check(struct tfm_sfn_req_s *desc_ptr)
             return tfm_core_ns_ipc_request(fn, (int32_t)a, (int32_t)b, \
                 (int32_t)c, (int32_t)d)
 
-__attribute__ ((naked))
+__attribute__ ((naked, section("SFN")))
 static int32_t tfm_core_ipc_request(const struct tfm_sfn_req_s *desc_ptr)
 {
     __ASM volatile("SVC %0           \n"
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.c b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.c
index 2559ac83b3..f589cd9a2f 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.c
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.c
@@ -32,6 +32,7 @@ typedef enum {
  * In case of an error in the error handling, a non-zero value have to be
  * returned.
  */
+#ifndef TFM_PSA_API
 static void tfm_spm_partition_err_handler(
     const struct spm_partition_desc_t *partition,
     sp_error_type_t err_type,
@@ -53,6 +54,7 @@ static void tfm_spm_partition_err_handler(
     tfm_spm_partition_set_state(partition->static_data.partition_id,
             SPM_PARTITION_STATE_CLOSED);
 }
+#endif /* !defined(TFM_PSA_API) */
 
 /*
  * This function prevents name clashes between the variable names accessibles in
@@ -98,29 +100,34 @@ enum spm_err_t tfm_spm_db_init(void)
      */
 
     /* For the non secure Execution environment */
+#if (TFM_LVL != 1) || defined(TFM_PSA_API)
     extern uint32_t Image$$ARM_LIB_STACK$$ZI$$Base[];
     extern uint32_t Image$$ARM_LIB_STACK$$ZI$$Limit[];
     uint32_t psp_stack_bottom = (uint32_t)Image$$ARM_LIB_STACK$$ZI$$Base;
     uint32_t psp_stack_top    = (uint32_t)Image$$ARM_LIB_STACK$$ZI$$Limit;
-
+#endif
     if (g_spm_partition_db.partition_count >= SPM_MAX_PARTITIONS) {
         return SPM_ERR_INVALID_CONFIG;
     }
     part_ptr = &(g_spm_partition_db.partitions[
             g_spm_partition_db.partition_count]);
     part_ptr->static_data.partition_id = TFM_SP_NON_SECURE_ID;
+#ifdef TFM_PSA_API
+    part_ptr->static_data.partition_flags = SPM_PART_FLAG_APP_ROT |
+                                            SPM_PART_FLAG_IPC;
+    part_ptr->static_data.partition_priority = TFM_PRIORITY_LOW;
+    part_ptr->static_data.partition_init = tfm_nspm_thread_entry;
+#else
     part_ptr->static_data.partition_flags = 0;
+#endif
 
-#if TFM_LVL != 1
+#if (TFM_LVL != 1) || defined(TFM_PSA_API)
     part_ptr->memory_data.stack_bottom = psp_stack_bottom;
     part_ptr->memory_data.stack_top    = psp_stack_top;
     /* Since RW, ZI and stack are configured as one MPU region, configure
      * RW start address to psp_stack_bottom to get RW access to stack
      */
     part_ptr->memory_data.rw_start     = psp_stack_bottom;
-#else
-    part_ptr->stack_limit = psp_stack_bottom;
-    part_ptr->stack_size = psp_stack_top - psp_stack_bottom;
 #endif
 
     part_ptr->runtime_data.partition_state = SPM_PARTITION_STATE_UNINIT;
@@ -149,6 +156,7 @@ enum spm_err_t tfm_spm_db_init(void)
     return SPM_ERR_OK;
 }
 
+#ifndef TFM_PSA_API
 enum spm_err_t tfm_spm_partition_init(void)
 {
     struct spm_partition_desc_t *part;
@@ -161,11 +169,6 @@ enum spm_err_t tfm_spm_partition_init(void)
     for (idx = 0; idx < g_spm_partition_db.partition_count; ++idx) {
         part = &g_spm_partition_db.partitions[idx];
         tfm_spm_hal_configure_default_isolation(part->platform_data);
-#ifdef TFM_PSA_API
-        if (part->static_data.partition_flags & SPM_PART_FLAG_IPC) {
-            continue;
-        }
-#endif
         if (part->static_data.partition_init == NULL) {
             tfm_spm_partition_set_state(idx, SPM_PARTITION_STATE_IDLE);
             tfm_spm_partition_set_caller_partition_idx(idx,
@@ -196,8 +199,22 @@ enum spm_err_t tfm_spm_partition_init(void)
         return SPM_ERR_PARTITION_NOT_AVAILABLE;
     }
 }
+#endif /* !defined(TFM_PSA_API) */
 
-#if TFM_LVL != 1
+#if (TFM_LVL != 1) || defined(TFM_PSA_API)
+uint32_t tfm_spm_partition_get_stack_bottom(uint32_t partition_idx)
+{
+    return g_spm_partition_db.partitions[partition_idx].
+            memory_data.stack_bottom;
+}
+
+uint32_t tfm_spm_partition_get_stack_top(uint32_t partition_idx)
+{
+    return g_spm_partition_db.partitions[partition_idx].memory_data.stack_top;
+}
+#endif
+
+#if (TFM_LVL != 1) && !defined(TFM_PSA_API)
 enum spm_err_t tfm_spm_partition_sandbox_config(uint32_t partition_idx)
 {
     struct spm_partition_desc_t *part;
@@ -226,17 +243,6 @@ enum spm_err_t tfm_spm_partition_sandbox_deconfig(uint32_t partition_idx)
                                                   part->platform_data);
 }
 
-uint32_t tfm_spm_partition_get_stack_bottom(uint32_t partition_idx)
-{
-    return g_spm_partition_db.partitions[partition_idx].
-            memory_data.stack_bottom;
-}
-
-uint32_t tfm_spm_partition_get_stack_top(uint32_t partition_idx)
-{
-    return g_spm_partition_db.partitions[partition_idx].memory_data.stack_top;
-}
-
 uint32_t tfm_spm_partition_get_zi_start(uint32_t partition_idx)
 {
     return g_spm_partition_db.partitions[partition_idx].
@@ -268,15 +274,6 @@ void tfm_spm_partition_set_stack(uint32_t partition_idx, uint32_t stack_ptr)
 }
 #endif
 
-void tfm_spm_partition_store_context(uint32_t partition_idx,
-        uint32_t stack_ptr, uint32_t lr)
-{
-    g_spm_partition_db.partitions[partition_idx].
-            runtime_data.stack_ptr = stack_ptr;
-    g_spm_partition_db.partitions[partition_idx].
-            runtime_data.lr = lr;
-}
-
 uint32_t tfm_spm_partition_get_partition_id(uint32_t partition_idx)
 {
     return g_spm_partition_db.partitions[partition_idx].static_data.
@@ -289,6 +286,16 @@ uint32_t tfm_spm_partition_get_flags(uint32_t partition_idx)
             partition_flags;
 }
 
+#ifndef TFM_PSA_API
+void tfm_spm_partition_store_context(uint32_t partition_idx,
+        uint32_t stack_ptr, uint32_t lr)
+{
+    g_spm_partition_db.partitions[partition_idx].
+            runtime_data.stack_ptr = stack_ptr;
+    g_spm_partition_db.partitions[partition_idx].
+            runtime_data.lr = lr;
+}
+
 const struct spm_partition_runtime_data_t *
             tfm_spm_partition_get_runtime_data(uint32_t partition_idx)
 {
@@ -390,3 +397,20 @@ void tfm_spm_partition_cleanup_context(uint32_t partition_idx)
     partition->runtime_data.orig_outvec = 0;
     partition->runtime_data.iovec_api = 0;
 }
+#endif /* !defined(TFM_PSA_API) */
+
+__attribute__((section("SFN")))
+void tfm_spm_partition_change_privilege(uint32_t privileged)
+{
+    CONTROL_Type ctrl;
+
+    ctrl.w = __get_CONTROL();
+
+    if (privileged == TFM_PARTITION_PRIVILEGED_MODE) {
+        ctrl.b.nPRIV = 0;
+    } else {
+        ctrl.b.nPRIV = 1;
+    }
+
+    __set_CONTROL(ctrl.w);
+}
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.h
index 67accd5e8a..a15434d43a 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_api.h
@@ -15,6 +15,10 @@
 
 #define SPM_INVALID_PARTITION_IDX     (~0U)
 
+/* Privileged definitions for partition thread mode */
+#define TFM_PARTITION_PRIVILEGED_MODE   1
+#define TFM_PARTITION_UNPRIVILEGED_MODE 0
+
 enum spm_err_t {
     SPM_ERR_OK = 0,
     SPM_ERR_PARTITION_DB_NOT_INIT,
@@ -83,29 +87,7 @@ struct spm_partition_runtime_data_t {
  */
 uint32_t get_partition_idx(uint32_t partition_id);
 
-#if TFM_LVL != 1
-/**
- * \brief Configure isolated sandbox for a partition
- *
- * \param[in] partition_idx     Partition index
- *
- * \return Error code \ref spm_err_t
- *
- * \note This function doesn't check if partition_idx is valid.
- */
-enum spm_err_t tfm_spm_partition_sandbox_config(uint32_t partition_idx);
-
-/**
- * \brief Deconfigure sandbox for a partition
- *
- * \param[in] partition_idx     Partition index
- *
- * \return Error code \ref spm_err_t
- *
- * \note This function doesn't check if partition_idx is valid.
- */
-enum spm_err_t tfm_spm_partition_sandbox_deconfig(uint32_t partition_idx);
-
+#if (TFM_LVL != 1) || defined(TFM_PSA_API)
 /**
  * \brief Get bottom of stack region for a partition
  *
@@ -127,6 +109,30 @@ uint32_t tfm_spm_partition_get_stack_bottom(uint32_t partition_idx);
  * \note This function doesn't check if partition_idx is valid.
  */
 uint32_t tfm_spm_partition_get_stack_top(uint32_t partition_idx);
+#endif
+
+#if (TFM_LVL != 1) && !defined(TFM_PSA_API)
+/**
+ * \brief Configure isolated sandbox for a partition
+ *
+ * \param[in] partition_idx     Partition index
+ *
+ * \return Error code \ref spm_err_t
+ *
+ * \note This function doesn't check if partition_idx is valid.
+ */
+enum spm_err_t tfm_spm_partition_sandbox_config(uint32_t partition_idx);
+
+/**
+ * \brief Deconfigure sandbox for a partition
+ *
+ * \param[in] partition_idx     Partition index
+ *
+ * \return Error code \ref spm_err_t
+ *
+ * \note This function doesn't check if partition_idx is valid.
+ */
+enum spm_err_t tfm_spm_partition_sandbox_deconfig(uint32_t partition_idx);
 
 /**
  * \brief Get the start of the zero-initialised region for a partition
@@ -185,6 +191,17 @@ uint32_t tfm_spm_partition_get_rw_limit(uint32_t partition_idx);
 void tfm_spm_partition_set_stack(uint32_t partition_idx, uint32_t stack_ptr);
 #endif
 
+/**
+ * \brief Get the id of the partition for its index from the db
+ *
+ * \param[in] partition_idx     Partition index
+ *
+ * \return Partition ID for that partition
+ *
+ * \note This function doesn't check if partition_idx is valid.
+ */
+uint32_t tfm_spm_partition_get_partition_id(uint32_t partition_idx);
+
 /**
  * \brief Get the flags associated with a partition
  *
@@ -196,6 +213,7 @@ void tfm_spm_partition_set_stack(uint32_t partition_idx, uint32_t stack_ptr);
  */
 uint32_t tfm_spm_partition_get_flags(uint32_t partition_idx);
 
+#ifndef TFM_PSA_API
 /**
  * \brief Get the current runtime data of a partition
  *
@@ -228,17 +246,6 @@ uint32_t tfm_spm_partition_get_running_partition_idx(void);
 void tfm_spm_partition_store_context(uint32_t partition_idx,
         uint32_t stack_ptr, uint32_t lr);
 
-/**
- * \brief Get the id of the partition for its index from the db
- *
- * \param[in] partition_idx     Partition index
- *
- * \return Partition ID for that partition
- *
- * \note This function doesn't check if partition_idx is valid.
- */
-uint32_t tfm_spm_partition_get_partition_id(uint32_t partition_idx);
-
 /**
  * \brief Set the current state of a partition
  *
@@ -307,13 +314,6 @@ enum spm_err_t tfm_spm_partition_set_share(uint32_t partition_idx,
 enum spm_err_t tfm_spm_partition_set_iovec(uint32_t partition_idx,
                                            const int32_t *args);
 
-/**
- * \brief Initialize partition database
- *
- * \return Error code \ref spm_err_t
- */
-enum spm_err_t tfm_spm_db_init(void);
-
 /**
  * \brief Execute partition init function
  *
@@ -329,5 +329,27 @@ enum spm_err_t tfm_spm_partition_init(void);
  * \note This function doesn't check if partition_idx is valid.
  */
 void tfm_spm_partition_cleanup_context(uint32_t partition_idx);
+#endif /* !defined(TFM_PSA_API) */
+
+/**
+ * \brief Initialize partition database
+ *
+ * \return Error code \ref spm_err_t
+ */
+enum spm_err_t tfm_spm_db_init(void);
+
+/**
+ * \brief Change the privilege mode for partition thread mode.
+ *
+ * \param[in] privileged        Privileged mode,
+ *                                \ref TFM_PARTITION_PRIVILEGED_MODE
+ *                                and \ref TFM_PARTITION_UNPRIVILEGED_MODE
+ *
+ * \note Barrier instructions are not called by this function, and if
+ *       it is called in thread mode, it might be necessary to call
+ *       them after this function returns (just like it is done in
+ *       jump_to_ns_code()).
+ */
+void tfm_spm_partition_change_privilege(uint32_t privileged);
 
 #endif /*__SPM_API_H__ */
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db.h
index 22d34ee8ff..dfdaf32ca1 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db.h
@@ -21,8 +21,6 @@ typedef psa_status_t(*sp_init_function)(void);
 #define TFM_PARTITION_TYPE_APP   "APPLICATION-ROT"
 #define TFM_PARTITION_TYPE_PSA   "PSA-ROT"
 
-#define TFM_STACK_SIZE           (1024 * 5)
-
 #ifdef TFM_PSA_API
 enum tfm_partition_priority {
     TFM_PRIORITY_LOW = THRD_PRIOR_LOWEST,
@@ -59,23 +57,22 @@ struct spm_partition_desc_t {
     struct spm_partition_static_data_t static_data;
     struct spm_partition_runtime_data_t runtime_data;
     struct tfm_spm_partition_platform_data_t *platform_data;
-#if TFM_LVL != 1
+#if (TFM_LVL != 1) || defined(TFM_PSA_API)
     struct tfm_spm_partition_memory_data_t memory_data;
 #endif
 #ifdef TFM_PSA_API
     struct tfm_thrd_ctx sp_thrd;
-    /*
-     * stack_limit points to starting address of the partitions' stack plus the partitions' stack size.
-     */
-    uint32_t stack_limit;
-    uint32_t stack_size;
 #endif
 };
 
 /* Macros to pick linker symbols and allow to form the partition data base */
 #define REGION(a, b, c) a##b##c
 #define REGION_NAME(a, b, c) REGION(a, b, c)
-#if TFM_LVL == 1
+/* Changed from #if (TFM_LVL == 1) && !defined(TFM_PSA_API) to #if (TFM_LVL == 1) to avoid linker error.
+   TF-M build autogenerates region details (code, ro, rw, zi and stack ) using linker scripts. We do not
+   hve that in mbed-os build yet.
+*/
+#if (TFM_LVL == 1)
 #define REGION_DECLARE(a, b, c)
 #else
 #define REGION_DECLARE(a, b, c) extern uint32_t REGION_NAME(a, b, c)
diff --git a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db_setup.h b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db_setup.h
index 099a800673..2cecfb78b0 100644
--- a/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db_setup.h
+++ b/components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/spm/spm_db_setup.h
@@ -38,7 +38,11 @@ struct spm_partition_db_t {
         data.partition_priority = TFM_PRIORITY(priority);                     \
     } while (0)
 
-#if TFM_LVL == 1
+/* Changed from #if (TFM_LVL == 1) && !defined(TFM_PSA_API) to #if (TFM_LVL == 1) to avoid linker error.
+   TF-M build autogenerates region details (code, ro, rw, zi and stack ) using linker scripts. We do not
+   hve that in mbed-os build yet.
+*/
+#if (TFM_LVL == 1)
 #define PARTITION_INIT_MEMORY_DATA(data, partition)
 #else
 #define PARTITION_INIT_MEMORY_DATA(data, partition)                            \
@@ -56,7 +60,6 @@ struct spm_partition_db_t {
     } while (0)
 #endif
 
-
 #if TFM_LVL == 1
 #define PARTITION_INIT_RUNTIME_DATA(data, partition)            \
     do {                                                        \
@@ -76,60 +79,60 @@ struct spm_partition_db_t {
     } while (0)
 #endif
 
-#define PARTITION_DECLARE(partition, flag, type, id, priority, part_stack_size)   \
-    do {                                                                     \
-        REGION_DECLARE(Image$$, partition, $$Base);                          \
-        REGION_DECLARE(Image$$, partition, $$Limit);                         \
-        REGION_DECLARE(Image$$, partition, $$RO$$Base);                      \
-        REGION_DECLARE(Image$$, partition, $$RO$$Limit);                     \
-        REGION_DECLARE(Image$$, partition, _DATA$$RW$$Base);                 \
-        REGION_DECLARE(Image$$, partition, _DATA$$RW$$Limit);                \
-        REGION_DECLARE(Image$$, partition, _DATA$$ZI$$Base);                 \
-        REGION_DECLARE(Image$$, partition, _DATA$$ZI$$Limit);                \
-        REGION_DECLARE(Image$$, partition, _STACK$$ZI$$Base);                \
-        REGION_DECLARE(Image$$, partition, _STACK$$ZI$$Limit);               \
-        int32_t flags = flag;                                                \
-        if (tfm_memcmp(type, TFM_PARTITION_TYPE_APP,                         \
-            strlen(TFM_PARTITION_TYPE_APP)) == 0) {                          \
-            flags |= SPM_PART_FLAG_APP_ROT;                                  \
-        } else if (tfm_memcmp(type, TFM_PARTITION_TYPE_PSA,                  \
-                   strlen(TFM_PARTITION_TYPE_PSA)) == 0) {                   \
-            flags |= SPM_PART_FLAG_PSA_ROT | SPM_PART_FLAG_APP_ROT;          \
-        } else {                                                             \
-            return SPM_ERR_INVALID_CONFIG;                                   \
-        }                                                                    \
-        struct spm_partition_desc_t *part_ptr;                               \
-        if (g_spm_partition_db.partition_count >= SPM_MAX_PARTITIONS) {      \
-            return SPM_ERR_INVALID_CONFIG;                                   \
-        }                                                                    \
-        __attribute__((section(".data.partitions_stacks")))                  \
-        static uint8_t partition##_stack[part_stack_size] __attribute__((aligned(8))); \
-        part_ptr = &(g_spm_partition_db.partitions[                          \
-            g_spm_partition_db.partition_count]);                            \
-        part_ptr->stack_limit = (uint32_t)partition##_stack;                 \
-        part_ptr->stack_size = part_stack_size;                              \
-        PARTITION_INIT_STATIC_DATA(part_ptr->static_data, partition, flags,  \
-                                   id, priority);                            \
-        PARTITION_INIT_RUNTIME_DATA(part_ptr->runtime_data, partition);      \
-        PARTITION_INIT_MEMORY_DATA(part_ptr->memory_data, partition);        \
-        ++g_spm_partition_db.partition_count;                                \
+#define PARTITION_DECLARE(partition, flag, type, id, priority, part_stack_size)                     \
+    do {                                                                                            \
+        REGION_DECLARE(Image$$, partition, $$Base);                                                 \
+        REGION_DECLARE(Image$$, partition, $$Limit);                                                \
+        REGION_DECLARE(Image$$, partition, $$RO$$Base);                                             \
+        REGION_DECLARE(Image$$, partition, $$RO$$Limit);                                            \
+        REGION_DECLARE(Image$$, partition, _DATA$$RW$$Base);                                        \
+        REGION_DECLARE(Image$$, partition, _DATA$$RW$$Limit);                                       \
+        REGION_DECLARE(Image$$, partition, _DATA$$ZI$$Base);                                        \
+        REGION_DECLARE(Image$$, partition, _DATA$$ZI$$Limit);                                       \
+        REGION_DECLARE(Image$$, partition, _STACK$$ZI$$Base);                                       \
+        REGION_DECLARE(Image$$, partition, _STACK$$ZI$$Limit);                                      \
+        int32_t flags = flag;                                                                       \
+        if (tfm_memcmp(type, TFM_PARTITION_TYPE_APP,                                                \
+            strlen(TFM_PARTITION_TYPE_APP)) == 0) {                                                 \
+            flags |= SPM_PART_FLAG_APP_ROT;                                                         \
+        } else if (tfm_memcmp(type, TFM_PARTITION_TYPE_PSA,                                         \
+                   strlen(TFM_PARTITION_TYPE_PSA)) == 0) {                                          \
+            flags |= SPM_PART_FLAG_PSA_ROT | SPM_PART_FLAG_APP_ROT;                                 \
+        } else {                                                                                    \
+            return SPM_ERR_INVALID_CONFIG;                                                          \
+        }                                                                                           \
+        struct spm_partition_desc_t *part_ptr;                                                      \
+        if (g_spm_partition_db.partition_count >= SPM_MAX_PARTITIONS) {                             \
+            return SPM_ERR_INVALID_CONFIG;                                                          \
+        }                                                                                           \
+        __attribute__((section(".data.partitions_stacks")))                                         \
+        static uint8_t partition##_stack[part_stack_size] __attribute__((aligned(8)));              \
+        part_ptr = &(g_spm_partition_db.partitions[                                                 \
+            g_spm_partition_db.partition_count]);                                                   \
+        part_ptr->memory_data.stack_bottom = (uint32_t)partition##_stack;                           \
+        part_ptr->memory_data.stack_top = part_ptr->memory_data.stack_bottom + part_stack_size;     \
+        PARTITION_INIT_STATIC_DATA(part_ptr->static_data, partition, flags,                         \
+                                   id, priority);                                                   \
+        PARTITION_INIT_RUNTIME_DATA(part_ptr->runtime_data, partition);                             \
+        PARTITION_INIT_MEMORY_DATA(part_ptr->memory_data, partition);                               \
+        ++g_spm_partition_db.partition_count;                                                       \
     } while (0)
 
-#define PARTITION_ADD_INIT_FUNC(partition, init_func)                     \
-    do {                                                                  \
-        extern int32_t init_func(void);                                   \
-        uint32_t partition_idx = get_partition_idx(partition##_ID);       \
-        struct spm_partition_desc_t *part_ptr =                           \
-            &(g_spm_partition_db.partitions[partition_idx]);              \
-        part_ptr->static_data.partition_init = init_func;                 \
+#define PARTITION_ADD_INIT_FUNC(partition, init_func)                                               \
+    do {                                                                                            \
+        extern int32_t init_func(void);                                                             \
+        uint32_t partition_idx = get_partition_idx(partition##_ID);                                 \
+        struct spm_partition_desc_t *part_ptr =                                                     \
+            &(g_spm_partition_db.partitions[partition_idx]);                                        \
+        part_ptr->static_data.partition_init = init_func;                                           \
     } while (0)
 
-#define PARTITION_ADD_PERIPHERAL(partition, peripheral)                    \
-    do {                                                                   \
-        uint32_t partition_idx = get_partition_idx(partition##_ID);        \
-        struct spm_partition_desc_t *part_ptr =                            \
-            &(g_spm_partition_db.partitions[partition_idx]);               \
-        part_ptr->platform_data = peripheral;                              \
+#define PARTITION_ADD_PERIPHERAL(partition, peripheral)                                             \
+    do {                                                                                            \
+        uint32_t partition_idx = get_partition_idx(partition##_ID);                                 \
+        struct spm_partition_desc_t *part_ptr =                                                     \
+            &(g_spm_partition_db.partitions[partition_idx]);                                        \
+        part_ptr->platform_data = peripheral;                                                       \
     } while (0)
 
 #endif /* __SPM_DB_SETUP_H__ */
diff --git a/tools/importer/tfm_importer.json b/tools/importer/tfm_importer.json
index 3d0ac1a22d..67d44d47f4 100644
--- a/tools/importer/tfm_importer.json
+++ b/tools/importer/tfm_importer.json
@@ -35,6 +35,10 @@
     {
       "src_file": "platform/include/tfm_spm_hal.h",
       "dest_file": "components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/platform/include/tfm_spm_hal.h"
+    },
+    {
+      "src_file": "secure_fw/ns_callable/tfm_psa_api_veneers.c",
+      "dest_file": "components/TARGET_PSA/TARGET_TFM/COMPONENT_SPE/secure_fw/ns_callable/tfm_psa_api_veneers.c"
     }
   ],
   "folders": [
@@ -64,12 +68,33 @@
     }
   ],
   "commit_sha": [
-    "fb068d2cb4e89cacf0e9f413075bb4b211f1484f",
-    "5d41a2aeae71b13f2763bea4e55899646291e0eb",
-    "9c1e080e39adc7211c8c2c12cd652da3dc124299",
-    "78ed87028718b1b926d847ff6fc2f91d44e53d6d",
-    "280715f9b74ab29459d81edaf02b39e7a6acb13c",
-    "ea81bf91c90ae23dd9de012bfd7498613be00601",
-    "5342015bb12a486a1c563175a8a7129f0737c925"
+    {
+      "sha": "11bff3f3cbfbd3e2c284e884d0066531e6b47d7e",
+      "msg": "TF-M patch: General modifications, Remove un-needed files, Disable printf and uart, Modify include paths, Guard macros from mbed_lib with ifndef"
+    },
+    {
+      "sha": "795e6418d0e73841868b351b605659a05c04e1f6",
+       "msg": "TF-M patch: Fix tfm_ns_lock_init issue (TF-M issue #239), Link to bug tracking: https://developer.trustedfirmware.org/T239"
+    },
+    {
+      "sha": "35938a407133fe0c20c25b6fae2836148d1adfca",
+      "msg": "TF-M patch: Fix service handles not cleared issue (TF-M issue #230), Link to bug tracking: https://developer.trustedfirmware.org/T230"
+    },
+    {
+      "sha": "910a402ce6c96b654cb6ae1a5b679e4f856c5419",
+      "msg": "TF-M patch: Fix tfm_psa_call_venner wrong argument type (TF-M issue #241), Link to bug tracking: https://developer.trustedfirmware.org/T241"
+    },
+    {
+      "sha": "cb748c5608cd68a1dbecde5b3b2c1488c3d0d17b",
+      "msg": "TF-M patch: Change #if TFM_PSA_API to #ifdef TFM_PSA_API to avoid compiler errors as mbed-cli only generates "-D" macros only for "macros" defined in targets.json, TF-M task link: https://developer.trustedfirmware.org/T396"
+    },
+    {
+      "sha": "9a5110561a60ec9f663079a25ec54f7ad0832743",
+      "msg": "TF-M patch: Remove secure_fw/core/tfm_func_api.c which is required only when TFM_PSA_API is not set"
+    },
+    {
+      "sha": "6e899b3cc98c3e1811a160df09abbccddb2fa014",
+      "msg": "TF-M patch/workaround related to (TF-M issue #T240), Link to bug tracking: https://developer.trustedfirmware.org/T240, The issue is fixed by TF-M team. However they autogenerate region details (code, ro, rw, zi and stack ) using linker scripts and in mbed-os we also autogenerate region details but using mix of service definition in json file and other template files."
+    }
   ]
 }