diff --git a/bl1/bl1_1/shared_lib/CMakeLists.txt b/bl1/bl1_1/shared_lib/CMakeLists.txt
index 3109cb0..6a6a030 100644
--- a/bl1/bl1_1/shared_lib/CMakeLists.txt
+++ b/bl1/bl1_1/shared_lib/CMakeLists.txt
@@ -1,5 +1,5 @@
 #-------------------------------------------------------------------------------
-# Copyright (c) 2021, Arm Limited. All rights reserved.
+# Copyright (c) 2021-2022, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -32,7 +32,7 @@
     PUBLIC
         bl1_1_shared_lib_interface
         platform_region_defs
-        tfm_fih_implementation
+        tfm_fih
     PRIVATE
         $<$<BOOL:${TFM_BL1_SOFTWARE_CRYPTO}>:bl1_1_crypto_mbedcrypto>
         $<$<AND:$<NOT:$<BOOL:${TFM_BL1_SOFTWARE_CRYPTO}>>,$<BOOL:${CRYPTO_HW_ACCELERATOR}>>:bl1_crypto_hw>
diff --git a/interface/include/config_impl.h.template b/interface/include/config_impl.h.template
index a635514..83ae49e 100644
--- a/interface/include/config_impl.h.template
+++ b/interface/include/config_impl.h.template
@@ -24,7 +24,11 @@
 
 #if CONFIG_TFM_SPM_BACKEND_IPC == 1
 /* Trustzone NS agent working stack size. */
+#if defined(TFM_FIH_PROFILE_ON) && TFM_LVL == 1
+#define {{"%-56s"|format("CONFIG_TFM_NS_AGENT_TZ_STACK_SIZE")}} 1256
+#else
 #define {{"%-56s"|format("CONFIG_TFM_NS_AGENT_TZ_STACK_SIZE")}} 1024
+#endif
 
 /* SPM re-uses Trustzone NS agent stack. */
 #define {{"%-56s"|format("CONFIG_TFM_SPM_THREAD_STACK_SIZE")}}     \
diff --git a/lib/fih/CMakeLists.txt b/lib/fih/CMakeLists.txt
index a03aff5..8e5f876 100644
--- a/lib/fih/CMakeLists.txt
+++ b/lib/fih/CMakeLists.txt
@@ -1,5 +1,5 @@
 #-------------------------------------------------------------------------------
-# Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+# Copyright (c) 2020-2022, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -8,9 +8,9 @@
 cmake_minimum_required(VERSION 3.15)
 
 add_library(tfm_fih_headers INTERFACE)
-add_library(tfm_fih_implementation STATIC)
+add_library(tfm_fih STATIC)
 
-target_sources(tfm_fih_implementation
+target_sources(tfm_fih
     PRIVATE
         src/fih.c
 )
@@ -20,7 +20,7 @@
         $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/inc>
 )
 
-target_link_libraries(tfm_fih_implementation
+target_link_libraries(tfm_fih
     PUBLIC
         tfm_fih_headers
         platform_common_interface
diff --git a/lib/fih/inc/fih.h b/lib/fih/inc/fih.h
index 4414ef2..8b105dd 100644
--- a/lib/fih/inc/fih.h
+++ b/lib/fih/inc/fih.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -483,6 +483,13 @@
         return ret; \
     } while (0)
 
+/*
+ * FIH return type macro changes the function return types to fih_int.
+ * All functions that need to be protected by FIH and called via FIH_CALL must
+ * return a fih_int type.
+ */
+#define FIH_RET_TYPE(type)    fih_int
+
 #else /* TFM_FIH_PROFILE_ON */
 typedef int32_t fih_int;
 
@@ -516,6 +523,8 @@
         return ret; \
     } while (0)
 
+#define FIH_RET_TYPE(type)    type
+
 #define FIH_PANIC do { \
         while(1) {}; \
     } while (0)
diff --git a/platform/CMakeLists.txt b/platform/CMakeLists.txt
index 4a2e8f1..76346cb 100755
--- a/platform/CMakeLists.txt
+++ b/platform/CMakeLists.txt
@@ -87,10 +87,11 @@
     PRIVATE
         psa_interface
         tfm_partition_defs
+        platform_crypto_keys
         $<$<BOOL:${PLATFORM_DEFAULT_ATTEST_HAL}>:tfm_sprt>
         $<$<BOOL:${TFM_PARTITION_CRYPTO}>:crypto_service_mbedcrypto>
         $<$<BOOL:${TFM_PARTITION_INITIAL_ATTESTATION}>:tfm_attestation_defs>
-        platform_crypto_keys
+        $<$<NOT:$<STREQUAL:${TFM_FIH_PROFILE},OFF>>:tfm_fih>
 )
 
 target_compile_definitions(platform_s
@@ -179,6 +180,7 @@
         PUBLIC
             platform_common_interface
             platform_region_defs
+            tfm_fih_headers
         PRIVATE
             bl2_hal
             mcuboot_config
@@ -243,7 +245,7 @@
         PUBLIC
             platform_bl1_interface
         PRIVATE
-            tfm_fih_implementation
+            tfm_fih
             tfm_fih_headers
             $<$<BOOL:${CRYPTO_HW_ACCELERATOR}>:bl1_crypto_hw>
             tfm_boot_status
diff --git a/platform/ext/target/arm/mps2/an521/CMakeLists.txt b/platform/ext/target/arm/mps2/an521/CMakeLists.txt
index 66b8d89..ef8d81b 100644
--- a/platform/ext/target/arm/mps2/an521/CMakeLists.txt
+++ b/platform/ext/target/arm/mps2/an521/CMakeLists.txt
@@ -15,6 +15,11 @@
         partition
 )
 
+target_link_libraries(platform_region_defs
+    INTERFACE
+        tfm_fih_headers
+)
+
 #========================= Platform common defs ===============================#
 
 # Specify the location of platform specific build dependencies.
diff --git a/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.c b/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.c
index 960ee09..a5a816e 100644
--- a/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.c
+++ b/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2017-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -12,8 +12,8 @@
  * FixMe:
  * This is a beta quality driver for MPU in v8M. To be finalized.
  */
-
-enum mpu_armv8m_error_t mpu_armv8m_enable(struct mpu_armv8m_dev_t *dev,
+FIH_RET_TYPE(enum mpu_armv8m_error_t) mpu_armv8m_enable(
+                                          struct mpu_armv8m_dev_t *dev,
                                           uint32_t privdef_en,
                                           uint32_t hfnmi_en)
 {
@@ -45,7 +45,7 @@
     __DSB();
     __ISB();
 
-    return MPU_ARMV8M_OK;
+    FIH_RET(fih_int_encode(MPU_ARMV8M_OK));
 }
 
 enum mpu_armv8m_error_t mpu_armv8m_disable(struct mpu_armv8m_dev_t *dev)
@@ -58,7 +58,7 @@
     return MPU_ARMV8M_OK;
 }
 
-enum mpu_armv8m_error_t mpu_armv8m_region_enable(
+FIH_RET_TYPE(enum mpu_armv8m_error_t) mpu_armv8m_region_enable(
                                 struct mpu_armv8m_dev_t *dev,
                                 struct mpu_armv8m_region_cfg_t *region_cfg)
 {
@@ -70,7 +70,7 @@
 
     /*FIXME : Add complete error checking*/
     if ((region_cfg->region_base & ~MPU_RBAR_BASE_Msk) != 0) {
-        return MPU_ARMV8M_ERROR;
+        FIH_RET(fih_int_encode(MPU_ARMV8M_ERROR));
     }
     /* region_limit doesn't need to be aligned but the scatter
      * file needs to be setup to ensure that partitions do not overlap.
@@ -106,10 +106,11 @@
     __DSB();
     __ISB();
 
-    return MPU_ARMV8M_OK;
+    FIH_RET(fih_int_encode(MPU_ARMV8M_OK));
 }
 
-enum mpu_armv8m_error_t mpu_armv8m_region_disable(struct mpu_armv8m_dev_t *dev,
+FIH_RET_TYPE(enum mpu_armv8m_error_t) mpu_armv8m_region_disable(
+                                                  struct mpu_armv8m_dev_t *dev,
                                                   uint32_t region_nr)
 {
 
@@ -129,16 +130,17 @@
     /*Restore main MPU control*/
     mpu->CTRL = ctrl_before;
 
-    return MPU_ARMV8M_OK;
+    FIH_RET(fih_int_encode(MPU_ARMV8M_OK));
 }
 
 enum mpu_armv8m_error_t mpu_armv8m_clean(struct mpu_armv8m_dev_t *dev)
 {
     MPU_Type *mpu = (MPU_Type *)dev->base;
     uint32_t i = (mpu->TYPE & MPU_TYPE_DREGION_Msk) >> MPU_TYPE_DREGION_Pos;
+    fih_int fih_rc = FIH_FAILURE;
 
     while (i > 0) {
-        mpu_armv8m_region_disable(dev, i - 1);
+        FIH_CALL(mpu_armv8m_region_disable, fih_rc, dev, i - 1);
         i--;
     }
 
diff --git a/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.h b/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.h
index 9dccafe..48e9991 100644
--- a/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.h
+++ b/platform/ext/target/arm/mps2/an521/native_drivers/mpu_armv8m_drv.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2017-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -11,6 +11,7 @@
 #include <stdint.h>
 
 #include "cmsis.h"
+#include "fih.h"
 
 #define PRIVILEGED_DEFAULT_ENABLE 1
 #define HARDFAULT_NMI_ENABLE      1
@@ -79,8 +80,8 @@
  *
  * \note This function doesn't check if dev is NULL.
  */
-
-enum mpu_armv8m_error_t mpu_armv8m_enable(struct mpu_armv8m_dev_t *dev,
+FIH_RET_TYPE(enum mpu_armv8m_error_t) mpu_armv8m_enable(
+                                          struct mpu_armv8m_dev_t *dev,
                                           uint32_t privdef_en,
                                           uint32_t hfnmi_en);
 
@@ -116,7 +117,7 @@
  *
  * \note This function doesn't check if dev is NULL.
  */
-enum mpu_armv8m_error_t mpu_armv8m_region_enable(
+FIH_RET_TYPE(enum mpu_armv8m_error_t) mpu_armv8m_region_enable(
                                 struct mpu_armv8m_dev_t *dev,
                                 struct mpu_armv8m_region_cfg_t *region_cfg);
 
@@ -130,7 +131,8 @@
  *
  * \note This function doesn't check if dev is NULL.
  */
-enum mpu_armv8m_error_t mpu_armv8m_region_disable(struct mpu_armv8m_dev_t *dev,
+FIH_RET_TYPE(enum mpu_armv8m_error_t) mpu_armv8m_region_disable(
+                                                  struct mpu_armv8m_dev_t *dev,
                                                   uint32_t region_nr);
 
 #endif /* __MPU_ARMV8M_DRV_H__ */
diff --git a/platform/ext/target/arm/mps2/an521/partition/region_defs.h b/platform/ext/target/arm/mps2/an521/partition/region_defs.h
index d9d192b..b821a7d 100755
--- a/platform/ext/target/arm/mps2/an521/partition/region_defs.h
+++ b/platform/ext/target/arm/mps2/an521/partition/region_defs.h
@@ -23,10 +23,14 @@
 #define BL2_MSP_STACK_SIZE      (0x0001800)
 
 #ifdef ENABLE_HEAP
-    #define S_HEAP_SIZE             (0x0000200)
+#define S_HEAP_SIZE             (0x0000200)
 #endif
 
+#ifdef TFM_FIH_PROFILE_ON
+#define S_MSP_STACK_SIZE        (0x00009C0)
+#else
 #define S_MSP_STACK_SIZE        (0x0000800)
+#endif
 #define S_PSP_STACK_SIZE        (0x0000800)
 
 #define NS_HEAP_SIZE            (0x0001000)
diff --git a/platform/ext/target/arm/mps2/an521/target_cfg.c b/platform/ext/target/arm/mps2/an521/target_cfg.c
index 5c2a940..68001dd 100644
--- a/platform/ext/target/arm/mps2/an521/target_cfg.c
+++ b/platform/ext/target/arm/mps2/an521/target_cfg.c
@@ -15,6 +15,7 @@
  */
 
 #include "cmsis.h"
+#include "fih.h"
 #include "target_cfg.h"
 #include "Driver_MPC.h"
 #include "platform_retarget_dev.h"
@@ -203,7 +204,7 @@
     return TFM_PLAT_ERR_SUCCESS;
 }
 
-enum tfm_plat_err_t init_debug(void)
+FIH_RET_TYPE(enum tfm_plat_err_t) init_debug(void)
 {
     volatile struct sysctrl_t *sys_ctrl =
                                        (struct sysctrl_t *)CMSDK_SYSCTRL_BASE_S;
@@ -240,7 +241,7 @@
      */
 #endif
 
-    return TFM_PLAT_ERR_SUCCESS;
+    FIH_RET(fih_int_encode(TFM_PLAT_ERR_SUCCESS));
 }
 
 /*----------------- NVIC interrupt target state to NS configuration ----------*/
@@ -381,7 +382,7 @@
 
 #define NR_SAU_INIT_STEP                 3
 
-void sau_and_idau_cfg(void)
+FIH_RET_TYPE(int32_t) sau_and_idau_cfg(void)
 {
     struct spctrl_def *spctrl = CMSDK_SPCTRL;
     uint32_t i;
@@ -399,6 +400,8 @@
 
     /* Allows SAU to define the code region as a NSC */
     spctrl->nsccfg |= NSCCFG_CODENSC;
+
+    FIH_RET(fih_int_encode(ARM_DRIVER_OK));
 }
 
 /*------------------- Memory configuration functions -------------------------*/
@@ -408,13 +411,13 @@
 #define NR_MPC_INIT_STEP                 6
 #endif
 
-int32_t mpc_init_cfg(void)
+FIH_RET_TYPE(int32_t) mpc_init_cfg(void)
 {
     int32_t ret = ARM_DRIVER_OK;
 
     ret = Driver_SRAM1_MPC.Initialize();
     if (ret != ARM_DRIVER_OK) {
-        return ret;
+        FIH_RET(fih_int_encode(ret));
     }
 
     ret = Driver_SRAM1_MPC.ConfigRegion(
@@ -422,7 +425,7 @@
                                       memory_regions.non_secure_partition_limit,
                                       ARM_MPC_ATTR_NONSECURE);
     if (ret != ARM_DRIVER_OK) {
-        return ret;
+        FIH_RET(fih_int_encode(ret));
     }
 
 #ifdef BL2
@@ -431,13 +434,13 @@
                                   memory_regions.secondary_partition_limit,
                                   ARM_MPC_ATTR_NONSECURE);
     if (ret != ARM_DRIVER_OK) {
-        return ret;
+        FIH_RET(fih_int_encode(ret));
     }
 #endif /* BL2 */
 
     ret = Driver_SRAM2_MPC.Initialize();
     if (ret != ARM_DRIVER_OK) {
-        return ret;
+        FIH_RET(fih_int_encode(ret));
     }
 
     ret = Driver_SRAM2_MPC.ConfigRegion(NS_DATA_START, NS_DATA_LIMIT,
@@ -448,18 +451,18 @@
                                         ARM_MPC_ATTR_NONSECURE);
 #endif
     if (ret != ARM_DRIVER_OK) {
-        return ret;
+        FIH_RET(fih_int_encode(ret));
     }
 
     /* Lock down the MPC configuration */
     ret = Driver_SRAM1_MPC.LockDown();
     if (ret != ARM_DRIVER_OK) {
-        return ret;
+        FIH_RET(fih_int_encode(ret));
     }
 
     ret = Driver_SRAM2_MPC.LockDown();
     if (ret != ARM_DRIVER_OK) {
-        return ret;
+        FIH_RET(fih_int_encode(ret));
     }
 
     /* Add barriers to assure the MPC configuration is done before continue
@@ -468,13 +471,13 @@
     __DSB();
     __ISB();
 
-    return ARM_DRIVER_OK;
+    FIH_RET(fih_int_encode(ARM_DRIVER_OK));
 }
 
 /*---------------------- PPC configuration functions -------------------------*/
 #define NR_PPC_INIT_STEP                 4
 
-void ppc_init_cfg(void)
+FIH_RET_TYPE(int32_t) ppc_init_cfg(void)
 {
     struct spctrl_def* spctrl = CMSDK_SPCTRL;
     struct nspctrl_def* nspctrl = CMSDK_NSPCTRL;
@@ -543,6 +546,8 @@
      * bus error instead of RAZ/WI
      */
     spctrl->secrespcfg |= 1U;
+
+    FIH_RET(fih_int_encode(ARM_DRIVER_OK));
 }
 
 void ppc_configure_to_non_secure(enum ppc_bank_e bank, uint16_t pos)
@@ -552,23 +557,29 @@
     ((uint32_t*)&(spctrl->ahbnsppc0))[bank] |= (1U << pos);
 }
 
-void ppc_configure_to_secure(enum ppc_bank_e bank, uint16_t pos)
+FIH_RET_TYPE(int32_t) ppc_configure_to_secure(enum ppc_bank_e bank, uint16_t pos)
 {
     /* Clear NS flag for peripheral to prevent NS access */
     struct spctrl_def* spctrl = CMSDK_SPCTRL;
     ((uint32_t*)&(spctrl->ahbnsppc0))[bank] &= ~(1U << pos);
+
+    FIH_RET(fih_int_encode(ARM_DRIVER_OK));
 }
 
-void ppc_en_secure_unpriv(enum ppc_bank_e bank, uint16_t pos)
+FIH_RET_TYPE(int32_t) ppc_en_secure_unpriv(enum ppc_bank_e bank, uint16_t pos)
 {
     struct spctrl_def* spctrl = CMSDK_SPCTRL;
     ((uint32_t*)&(spctrl->ahbspppc0))[bank] |= (1U << pos);
+
+    FIH_RET(fih_int_encode(ARM_DRIVER_OK));
 }
 
-void ppc_clr_secure_unpriv(enum ppc_bank_e bank, uint16_t pos)
+FIH_RET_TYPE(int32_t) ppc_clr_secure_unpriv(enum ppc_bank_e bank, uint16_t pos)
 {
     struct spctrl_def* spctrl = CMSDK_SPCTRL;
     ((uint32_t*)&(spctrl->ahbspppc0))[bank] &= ~(1U << pos);
+
+    FIH_RET(fih_int_encode(ARM_DRIVER_OK));
 }
 
 void ppc_clear_irq(void)
diff --git a/platform/ext/target/arm/mps2/an521/target_cfg.h b/platform/ext/target/arm/mps2/an521/target_cfg.h
index 1a2feb4..d8794ba 100644
--- a/platform/ext/target/arm/mps2/an521/target_cfg.h
+++ b/platform/ext/target/arm/mps2/an521/target_cfg.h
@@ -21,6 +21,7 @@
 #include "tfm_peripherals_def.h"
 #include "tfm_plat_defs.h"
 #include "arm_uart_drv.h"
+#include "fih.h"
 
 #define TFM_DRIVER_STDIO    Driver_USART0
 #define NS_DRIVER_STDIO     Driver_USART0
@@ -81,17 +82,17 @@
  *
  * \return  Returns error code.
  */
-int32_t mpc_init_cfg(void);
+FIH_RET_TYPE(int32_t) mpc_init_cfg(void);
 
 /**
  * \brief Configures the Peripheral Protection Controller.
  */
-void ppc_init_cfg(void);
+FIH_RET_TYPE(int32_t) ppc_init_cfg(void);
 
 /**
  * \brief Restict access to peripheral to secure
  */
-void ppc_configure_to_secure(enum ppc_bank_e bank, uint16_t loc);
+FIH_RET_TYPE(int32_t) ppc_configure_to_secure(enum ppc_bank_e bank, uint16_t loc);
 
 /**
  * \brief Allow non-secure access to peripheral
@@ -101,12 +102,12 @@
 /**
  * \brief Enable secure unprivileged access to peripheral
  */
-void ppc_en_secure_unpriv(enum ppc_bank_e bank, uint16_t pos);
+FIH_RET_TYPE(int32_t) ppc_en_secure_unpriv(enum ppc_bank_e bank, uint16_t pos);
 
 /**
  * \brief Clear secure unprivileged access to peripheral
  */
-void ppc_clr_secure_unpriv(enum ppc_bank_e bank, uint16_t pos);
+FIH_RET_TYPE(int32_t) ppc_clr_secure_unpriv(enum ppc_bank_e bank, uint16_t pos);
 
 /**
  * \brief Clears PPC interrupt.
@@ -116,7 +117,7 @@
 /**
  * \brief Configures SAU and IDAU.
  */
-void sau_and_idau_cfg(void);
+FIH_RET_TYPE(int32_t) sau_and_idau_cfg(void);
 
 /**
  * \brief Enables the fault handlers and sets priorities.
@@ -137,7 +138,7 @@
  *
  * \return Returns values as specified by the \ref tfm_plat_err_t
  */
-enum tfm_plat_err_t init_debug(void);
+FIH_RET_TYPE(enum tfm_plat_err_t) init_debug(void);
 
 /**
  * \brief Configures all external interrupts to target the
diff --git a/platform/ext/target/arm/mps2/an521/tfm_hal_isolation.c b/platform/ext/target/arm/mps2/an521/tfm_hal_isolation.c
index 6167b9c..1b00101 100644
--- a/platform/ext/target/arm/mps2/an521/tfm_hal_isolation.c
+++ b/platform/ext/target/arm/mps2/an521/tfm_hal_isolation.c
@@ -155,14 +155,22 @@
 #endif /* TFM_LVL == 3 */
 #endif /* CONFIG_TFM_ENABLE_MEMORY_PROTECT */
 
-enum tfm_hal_status_t tfm_hal_set_up_static_boundaries(void)
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_set_up_static_boundaries(void)
 {
+    fih_int fih_rc = FIH_FAILURE;
     /* Set up isolation boundaries between SPE and NSPE */
-    sau_and_idau_cfg();
-    if (mpc_init_cfg() != ARM_DRIVER_OK) {
-        return TFM_HAL_ERROR_GENERIC;
+    FIH_CALL(sau_and_idau_cfg, fih_rc);
+    if (fih_not_eq(fih_rc, fih_int_encode(ARM_DRIVER_OK))) {
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
-    ppc_init_cfg();
+    FIH_CALL(mpc_init_cfg, fih_rc);
+    if (fih_not_eq(fih_rc, fih_int_encode(ARM_DRIVER_OK))) {
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
+    }
+    FIH_CALL(ppc_init_cfg, fih_rc);
+    if (fih_not_eq(fih_rc, fih_int_encode(ARM_DRIVER_OK))) {
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
+    }
 
     /* Set up static isolation boundaries inside SPE */
 #ifdef CONFIG_TFM_ENABLE_MEMORY_PROTECT
@@ -172,7 +180,7 @@
     mpu_armv8m_clean(&dev_mpu_s);
 
     if ((ARRAY_SIZE(region_cfg) + MIN_NR_PRIVATE_DATA_REGION) > MPU_REGION_NUM) {
-        return TFM_HAL_ERROR_GENERIC;
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
 
     /* Update MPU region numbers. The numbers start from 0 and are continuous. */
@@ -181,21 +189,22 @@
         /* Update region number */
         localcfg.region_nr = i;
         /* Enable regions */
-        if (mpu_armv8m_region_enable(&dev_mpu_s, &localcfg) != MPU_ARMV8M_OK) {
-            return TFM_HAL_ERROR_GENERIC;
+        FIH_CALL(mpu_armv8m_region_enable, fih_rc, &dev_mpu_s, &localcfg);
+        if (fih_not_eq(fih_rc, fih_int_encode(MPU_ARMV8M_OK))) {
+            FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
         }
     }
     n_configured_regions = i;
 
     /* Enable MPU */
-    if (mpu_armv8m_enable(&dev_mpu_s,
-                          PRIVILEGED_DEFAULT_ENABLE,
-                          HARDFAULT_NMI_ENABLE) != MPU_ARMV8M_OK) {
-        return TFM_HAL_ERROR_GENERIC;
+    FIH_CALL(mpu_armv8m_enable, fih_rc, &dev_mpu_s,
+             PRIVILEGED_DEFAULT_ENABLE, HARDFAULT_NMI_ENABLE);
+    if (fih_not_eq(fih_rc, fih_int_encode(MPU_ARMV8M_OK))) {
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
 #endif /* CONFIG_TFM_ENABLE_MEMORY_PROTECT */
 
-    return TFM_HAL_SUCCESS;
+    FIH_RET(fih_int_encode(TFM_HAL_SUCCESS));
 }
 
 #ifdef TFM_PSA_API
@@ -234,7 +243,7 @@
  * 1. The maximum number of allowed MMIO regions is 5.
  * 2. Highest 8 bits are for index. It supports 256 unique handles at most.
  */
-enum tfm_hal_status_t tfm_hal_bind_boundary(
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_bind_boundary(
                                     const struct partition_load_info_t *p_ldinf,
                                     uintptr_t *p_boundary)
 {
@@ -247,9 +256,10 @@
 #if TFM_LVL == 2
     struct mpu_armv8m_region_cfg_t localcfg;
 #endif
+    fih_int fih_rc = FIH_FAILURE;
 
     if (!p_ldinf || !p_boundary) {
-        return TFM_HAL_ERROR_GENERIC;
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
 
 #if TFM_LVL == 1
@@ -279,21 +289,33 @@
 
         if (j == ARRAY_SIZE(partition_named_mmio_list)) {
             /* The MMIO asset is not in the allowed list of platform. */
-            return TFM_HAL_ERROR_GENERIC;
+            FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
         }
         /* Assume PPC & MPC settings are required even under level 1 */
         plat_data_ptr = REFERENCE_TO_PTR(p_asset[i].dev.dev_ref,
                                          struct platform_data_t *);
 
         if (plat_data_ptr->periph_ppc_bank != PPC_SP_DO_NOT_CONFIGURE) {
-            ppc_configure_to_secure(plat_data_ptr->periph_ppc_bank,
-                                    plat_data_ptr->periph_ppc_loc);
+            FIH_CALL(ppc_configure_to_secure, fih_rc,
+                     plat_data_ptr->periph_ppc_bank,
+                     plat_data_ptr->periph_ppc_loc);
+            if (fih_not_eq(fih_rc, fih_int_encode(ARM_DRIVER_OK))) {
+                FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
+            }
             if (privileged) {
-                ppc_clr_secure_unpriv(plat_data_ptr->periph_ppc_bank,
-                                      plat_data_ptr->periph_ppc_loc);
+                FIH_CALL(ppc_clr_secure_unpriv, fih_rc,
+                         plat_data_ptr->periph_ppc_bank,
+                         plat_data_ptr->periph_ppc_loc);
+                if (fih_not_eq(fih_rc, fih_int_encode(ARM_DRIVER_OK))) {
+                    FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
+                }
             } else {
-                ppc_en_secure_unpriv(plat_data_ptr->periph_ppc_bank,
-                                      plat_data_ptr->periph_ppc_loc);
+                FIH_CALL(ppc_en_secure_unpriv, fih_rc,
+                         plat_data_ptr->periph_ppc_bank,
+                         plat_data_ptr->periph_ppc_loc);
+                if (fih_not_eq(fih_rc, fih_int_encode(ARM_DRIVER_OK))) {
+                    FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
+                }
             }
         }
 #if TFM_LVL == 2
@@ -310,9 +332,9 @@
             localcfg.attr_exec = MPU_ARMV8M_XN_EXEC_NEVER;
             localcfg.region_nr = n_configured_regions++;
 
-            if (mpu_armv8m_region_enable(&dev_mpu_s, &localcfg)
-                != MPU_ARMV8M_OK) {
-                return TFM_HAL_ERROR_GENERIC;
+            FIH_CALL(mpu_armv8m_region_enable, fih_rc, &dev_mpu_s, &localcfg);
+            if (fih_not_eq(fih_rc, fih_int_encode(MPU_ARMV8M_OK))) {
+                FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
             }
         }
 #elif TFM_LVL == 3
@@ -332,7 +354,7 @@
      * must have exceeded the limit of 5.
      */
     if (partition_attrs & HANDLE_INDEX_MASK) {
-        return TFM_HAL_ERROR_GENERIC;
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
     HANDLE_ENCODE_INDEX(partition_attrs, idx_boundary_handle);
 #endif
@@ -343,10 +365,10 @@
                         HANDLE_ATTR_NS_MASK;
     *p_boundary = (uintptr_t)partition_attrs;
 
-    return TFM_HAL_SUCCESS;
+    FIH_RET(fih_int_encode(TFM_HAL_SUCCESS));
 }
 
-enum tfm_hal_status_t tfm_hal_activate_boundary(
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_activate_boundary(
                              const struct partition_load_info_t *p_ldinf,
                              uintptr_t boundary)
 {
@@ -358,7 +380,8 @@
     uint32_t i, mmio_index;
     struct platform_data_t *plat_data_ptr;
     struct asset_desc_t *rt_mem;
-#endif
+#endif /* TFM_LVL == 3 */
+    fih_int fih_rc = FIH_FAILURE;
 
     /* Privileged level is required to be set always */
     ctrl.w = __get_CONTROL();
@@ -367,12 +390,12 @@
 
 #if TFM_LVL == 3
     if (!p_ldinf) {
-        return TFM_HAL_ERROR_GENERIC;
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
 
     /* Update regions, for unprivileged partitions only */
     if (privileged) {
-        return TFM_HAL_SUCCESS;
+        FIH_RET(fih_int_encode(TFM_HAL_SUCCESS));
     }
 
     /* Setup runtime memory first */
@@ -392,8 +415,9 @@
         localcfg.region_base = rt_mem[i].mem.start;
         localcfg.region_limit = rt_mem[i].mem.limit;
 
-        if (mpu_armv8m_region_enable(&dev_mpu_s, &localcfg) != MPU_ARMV8M_OK) {
-            return TFM_HAL_ERROR_GENERIC;
+        FIH_CALL(mpu_armv8m_region_enable, fih_rc, &dev_mpu_s, &localcfg);
+        if (fih_not_eq(fih_rc, fih_int_encode(MPU_ARMV8M_OK))) {
+            FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
         }
     }
 
@@ -415,8 +439,9 @@
         localcfg.region_base = plat_data_ptr->periph_start;
         localcfg.region_limit = plat_data_ptr->periph_limit;
 
-        if (mpu_armv8m_region_enable(&dev_mpu_s, &localcfg) != MPU_ARMV8M_OK) {
-            return TFM_HAL_ERROR_GENERIC;
+        FIH_CALL(mpu_armv8m_region_enable, fih_rc, &dev_mpu_s, &localcfg);
+        if (fih_not_eq(fih_rc, fih_int_encode(MPU_ARMV8M_OK))) {
+            FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
         }
 
         local_handle >>= HANDLE_PER_ATTR_BITS;
@@ -425,27 +450,29 @@
 
     /* Disable unused regions */
     while (i < MPU_REGION_NUM) {
-        if (mpu_armv8m_region_disable(&dev_mpu_s, i++)!= MPU_ARMV8M_OK) {
-            return TFM_HAL_ERROR_GENERIC;
+        FIH_CALL(mpu_armv8m_region_disable, fih_rc, &dev_mpu_s, i++);
+        if (fih_not_eq(fih_rc, fih_int_encode(MPU_ARMV8M_OK))) {
+            FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
         }
     }
-#endif
-    return TFM_HAL_SUCCESS;
+#endif /* TFM_LVL == 3 */
+    FIH_RET(fih_int_encode(TFM_HAL_SUCCESS));
 }
 #endif /* TFM_PSA_API */
 
-enum tfm_hal_status_t tfm_hal_memory_check(uintptr_t boundary, uintptr_t base,
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_memory_check(
+                                           uintptr_t boundary, uintptr_t base,
                                            size_t size, uint32_t access_type)
 {
     int flags = 0;
 
     /* If size is zero, this indicates an empty buffer and base is ignored */
     if (size == 0) {
-        return TFM_HAL_SUCCESS;
+        FIH_RET(fih_int_encode(TFM_HAL_SUCCESS));
     }
 
     if (!base) {
-        return TFM_HAL_ERROR_INVALID_INPUT;
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_INVALID_INPUT));
     }
 
     if ((access_type & TFM_HAL_ACCESS_READWRITE) == TFM_HAL_ACCESS_READWRITE) {
@@ -453,7 +480,7 @@
     } else if (access_type & TFM_HAL_ACCESS_READABLE) {
         flags |= CMSE_MPU_READ;
     } else {
-        return TFM_HAL_ERROR_INVALID_INPUT;
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_INVALID_INPUT));
     }
 
     if (!((uint32_t)boundary & HANDLE_ATTR_PRIV_MASK)) {
@@ -472,8 +499,8 @@
     }
 
     if (cmse_check_address_range((void *)base, size, flags) != NULL) {
-        return TFM_HAL_SUCCESS;
+        FIH_RET(fih_int_encode(TFM_HAL_SUCCESS));
     } else {
-        return TFM_HAL_ERROR_MEM_FAULT;
+        FIH_RET(fih_int_encode(TFM_HAL_ERROR_MEM_FAULT));
     }
 }
diff --git a/platform/ext/target/arm/mps2/an521/tfm_hal_platform.c b/platform/ext/target/arm/mps2/an521/tfm_hal_platform.c
index f0d303e..3e2eaf6 100644
--- a/platform/ext/target/arm/mps2/an521/tfm_hal_platform.c
+++ b/platform/ext/target/arm/mps2/an521/tfm_hal_platform.c
@@ -16,16 +16,10 @@
 
 extern const struct memory_region_limits memory_regions;
 
-#ifdef TFM_FIH_PROFILE_ON
-fih_int tfm_hal_platform_init(void)
-#else
-enum tfm_hal_status_t tfm_hal_platform_init(void)
-#endif
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_platform_init(void)
 {
     enum tfm_plat_err_t plat_err = TFM_PLAT_ERR_SYSTEM_ERR;
-#ifdef TFM_FIH_PROFILE_ON
     fih_int fih_rc = FIH_FAILURE;
-#endif
 
     plat_err = enable_fault_handlers();
     if (plat_err != TFM_PLAT_ERR_SUCCESS) {
@@ -37,17 +31,10 @@
         FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
 
-#ifdef TFM_FIH_PROFILE_ON
     FIH_CALL(init_debug, fih_rc);
     if (fih_not_eq(fih_rc, fih_int_encode(TFM_PLAT_ERR_SUCCESS))) {
         FIH_RET(fih_int_encode(TFM_HAL_ERROR_GENERIC));
     }
-#else
-    plat_err = init_debug();
-    if (plat_err != TFM_PLAT_ERR_SUCCESS) {
-        return TFM_HAL_ERROR_GENERIC;
-    }
-#endif
 
     __enable_irq();
     stdio_init();
diff --git a/platform/include/tfm_hal_isolation.h b/platform/include/tfm_hal_isolation.h
index 835155a..a041d9a 100644
--- a/platform/include/tfm_hal_isolation.h
+++ b/platform/include/tfm_hal_isolation.h
@@ -10,6 +10,7 @@
 
 #include <stddef.h>
 #include <stdint.h>
+#include "fih.h"
 #include "tfm_hal_defs.h"
 #include "load/partition_defs.h"
 #include "load/asset_defs.h"
@@ -30,16 +31,6 @@
         (TFM_HAL_ACCESS_READABLE | TFM_HAL_ACCESS_WRITABLE)
 
 #ifdef TFM_FIH_PROFILE_ON
-#include "fih.h"
-/**
- * \brief  Sets up the static isolation boundaries which are constant throughout
- *         the runtime of the system, including the SPE/NSPE and partition
- *         boundaries.
- *
- * \return TFM_HAL_SUCCESS - the isolation boundaries have been set up.
- *         TFM_HAL_ERROR_GENERIC - failed to set up the isolation boundaries.
- */
-fih_int tfm_hal_set_up_static_boundaries(void);
 
 /**
  * \brief This function is responsible for checking all critical isolation
@@ -50,21 +41,7 @@
  */
 fih_int tfm_hal_verify_static_boundaries(void);
 
-/**
- * \brief  Activate one Secure Partition boundary.
- *
- * \param[in]   p_ldinf         Partition load information.
- * \param[in]   boundary        Platform boundary value for partition.
- *
- * \return TFM_HAL_SUCCESS          The isolation boundaries update succeeded.
- *         TFM_HAL_ERROR_GENERIC    Failed to update the isolation boundaries.
- *
- * \note   When FIH_ENABLE_DOUBLE_VARS is enabled, the return code will be
- *         wrapped and protected in \ref fih_int structure.
- */
-fih_int tfm_hal_activate_boundary(const struct partition_load_info_t *p_ldinf,
-                                  uintptr_t boundary);
-#else /* TFM_FIH_PROFILE_ON */
+#endif /* TFM_FIH_PROFILE_ON */
 /**
  * \brief  Sets up the static isolation boundaries which are constant throughout
  *         the runtime of the system, including the SPE/NSPE and partition
@@ -73,7 +50,7 @@
  * \return TFM_HAL_SUCCESS - the isolation boundaries have been set up.
  *         TFM_HAL_ERROR_GENERIC - failed to set up the isolation boundaries.
  */
-enum tfm_hal_status_t tfm_hal_set_up_static_boundaries(void);
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_set_up_static_boundaries(void);
 
 /**
  * \brief  Activate one Secure Partition boundary.
@@ -84,10 +61,9 @@
  * \return TFM_HAL_SUCCESS          The isolation boundaries update succeeded.
  *         TFM_HAL_ERROR_GENERIC    Failed to update the isolation boundaries.
  */
-enum tfm_hal_status_t tfm_hal_activate_boundary(
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_activate_boundary(
                             const struct partition_load_info_t *p_ldinf,
                             uintptr_t boundary);
-#endif /* TFM_FIH_PROFILE_ON */
 
 /**
  * \brief  This API checks if a given range of memory can be accessed with
@@ -107,7 +83,8 @@
  *         TFM_HAL_ERROR_INVALID_INPUT - Invalid inputs.
  *         TFM_HAL_ERROR_GENERIC - An error occurred.
  */
-enum tfm_hal_status_t tfm_hal_memory_check(uintptr_t boundary, uintptr_t base,
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_memory_check(
+                                           uintptr_t boundary, uintptr_t base,
                                            size_t size, uint32_t access_type);
 
 /**
@@ -128,7 +105,7 @@
  * \return TFM_HAL_SUCCESS          - A platform value bound successfully.
  *         TFM_HAL_ERROR_GENERIC    - Error occurred while binding.
  */
-enum tfm_hal_status_t tfm_hal_bind_boundary(
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_bind_boundary(
                                     const struct partition_load_info_t *p_ldinf,
                                     uintptr_t *p_boundary);
 
diff --git a/platform/include/tfm_hal_platform.h b/platform/include/tfm_hal_platform.h
index 6e7f5b0..c03c14b 100644
--- a/platform/include/tfm_hal_platform.h
+++ b/platform/include/tfm_hal_platform.h
@@ -16,18 +16,6 @@
 #include "fih.h"
 #include "tfm_hal_defs.h"
 
-#ifdef TFM_FIH_PROFILE_ON
-
-/**
- * \brief This function performs the platform-specific initialization.
- *
- * This function is called after architecture and platform common initialization
- * has finished during system early startup.
- *
- * \retval Returns values as specified by FIH specific platform error code.
- */
-fih_int tfm_hal_platform_init(void);
-#else
 /**
  * \brief This function performs the platform-specific initialization.
  *
@@ -37,8 +25,7 @@
  * \retval TFM_HAL_SUCCESS          Init success.
  * \retval TFM_HAL_ERROR_GENERIC    Generic errors.
  */
-enum tfm_hal_status_t tfm_hal_platform_init(void);
-#endif
+FIH_RET_TYPE(enum tfm_hal_status_t) tfm_hal_platform_init(void);
 
 /**
  * \brief System reset
diff --git a/platform/ns/CMakeLists.txt b/platform/ns/CMakeLists.txt
index d2b3bf2..c237288 100755
--- a/platform/ns/CMakeLists.txt
+++ b/platform/ns/CMakeLists.txt
@@ -29,6 +29,7 @@
     PUBLIC
         platform_common_interface
         platform_region_defs
+        tfm_fih_headers
         tfm_ns_interface
 )
 
diff --git a/secure_fw/spm/CMakeLists.txt b/secure_fw/spm/CMakeLists.txt
index 1812821..ce02d19 100755
--- a/secure_fw/spm/CMakeLists.txt
+++ b/secure_fw/spm/CMakeLists.txt
@@ -93,6 +93,7 @@
         tfm_partitions
         tfm_fih_headers
         tfm_sprt
+        $<$<NOT:$<STREQUAL:${TFM_FIH_PROFILE},OFF>>:tfm_fih>
 )
 
 target_compile_definitions(tfm_spm
diff --git a/secure_fw/spm/cmsis_psa/main.c b/secure_fw/spm/cmsis_psa/main.c
index b74bfc4..937322e 100644
--- a/secure_fw/spm/cmsis_psa/main.c
+++ b/secure_fw/spm/cmsis_psa/main.c
@@ -23,27 +23,16 @@
 static fih_int tfm_core_init(void)
 {
     enum tfm_plat_err_t plat_err = TFM_PLAT_ERR_SYSTEM_ERR;
-#ifdef TFM_FIH_PROFILE_ON
     fih_int fih_rc = FIH_FAILURE;
-#else
-    enum tfm_hal_status_t hal_status = TFM_HAL_ERROR_GENERIC;
-#endif
 
     /*
      * Access to any peripheral should be performed after programming
      * the necessary security components such as PPC/SAU.
      */
-#ifdef TFM_FIH_PROFILE_ON
     FIH_CALL(tfm_hal_set_up_static_boundaries, fih_rc);
     if (fih_not_eq(fih_rc, fih_int_encode(TFM_HAL_SUCCESS))) {
         FIH_RET(fih_int_encode(TFM_ERROR_GENERIC));
     }
-#else /* TFM_FIH_PROFILE_ON */
-    hal_status = tfm_hal_set_up_static_boundaries();
-    if (hal_status != TFM_HAL_SUCCESS) {
-        return TFM_ERROR_GENERIC;
-    }
-#endif /* TFM_FIH_PROFILE_ON */
 
 #ifdef TFM_FIH_PROFILE_ON
     FIH_CALL(tfm_hal_verify_static_boundaries, fih_rc);
@@ -52,21 +41,14 @@
     }
 #endif
 
-#ifdef TFM_FIH_PROFILE_ON
     FIH_CALL(tfm_hal_platform_init, fih_rc);
     if (fih_not_eq(fih_rc, fih_int_encode(TFM_HAL_SUCCESS))) {
         FIH_RET(fih_int_encode(TFM_ERROR_GENERIC));
     }
-#else /* TFM_FIH_PROFILE_ON */
-    hal_status = tfm_hal_platform_init();
-    if (hal_status != TFM_HAL_SUCCESS) {
-        return TFM_ERROR_GENERIC;
-    }
-#endif /* TFM_FIH_PROFILE_ON */
 
     plat_err = tfm_plat_otp_init();
     if (plat_err != TFM_PLAT_ERR_SUCCESS) {
-            FIH_RET(fih_int_encode(TFM_ERROR_GENERIC));
+        FIH_RET(fih_int_encode(TFM_ERROR_GENERIC));
     }
 
     /* Perform provisioning. */
diff --git a/secure_fw/spm/cmsis_psa/spm_ipc.c b/secure_fw/spm/cmsis_psa/spm_ipc.c
index 3ff9499..19ade00 100755
--- a/secure_fw/spm/cmsis_psa/spm_ipc.c
+++ b/secure_fw/spm/cmsis_psa/spm_ipc.c
@@ -451,10 +451,7 @@
 {
     struct partition_t *partition;
     uint32_t service_setting;
-
-#ifdef TFM_FIH_PROFILE_ON
     fih_int fih_rc = FIH_FAILURE;
-#endif
 
     tfm_pool_init(conn_handle_pool,
                   POOL_BUFFER_SIZE(conn_handle_pool),
@@ -482,19 +479,11 @@
         load_irqs_assuredly(partition);
 
         /* Bind the partition with platform. */
-#if TFM_FIH_PROFILE_ON
         FIH_CALL(tfm_hal_bind_boundary, fih_rc, partition->p_ldinf,
                  &partition->boundary);
         if (fih_not_eq(fih_rc, fih_int_encode(TFM_HAL_SUCCESS))) {
             tfm_core_panic();
         }
-#else /* TFM_FIH_PROFILE_ON */
-        if (tfm_hal_bind_boundary(partition->p_ldinf,
-                                    &partition->boundary)
-                != TFM_HAL_SUCCESS) {
-            tfm_core_panic();
-        }
-#endif /* TFM_FIH_PROFILE_ON */
 
         backend_init_comp_assuredly(partition, service_setting);
     }
diff --git a/secure_fw/spm/ffm/backend_ipc.c b/secure_fw/spm/ffm/backend_ipc.c
index 6e72b90..4123e29 100644
--- a/secure_fw/spm/ffm/backend_ipc.c
+++ b/secure_fw/spm/ffm/backend_ipc.c
@@ -197,6 +197,7 @@
 {
     uint32_t control;
     struct partition_t *p_cur_pt;
+    fih_int fih_rc = FIH_FAILURE;
 
 #if CONFIG_TFM_PSA_API_CROSS_CALL == 1
     TFM_CORE_ASSERT(SPM_THREAD_CONTEXT);
@@ -208,8 +209,8 @@
     p_cur_pt = TO_CONTAINER(CURRENT_THREAD->p_context_ctrl,
                             struct partition_t, ctx_ctrl);
 
-    if (tfm_hal_activate_boundary(p_cur_pt->p_ldinf, p_cur_pt->boundary)
-            != TFM_HAL_SUCCESS) {
+    FIH_CALL(tfm_hal_activate_boundary, fih_rc, p_cur_pt->p_ldinf, p_cur_pt->boundary);
+    if (fih_not_eq(fih_rc, fih_int_encode(TFM_HAL_SUCCESS))) {
         tfm_core_panic();
     }
 
@@ -249,6 +250,7 @@
 
 uint64_t ipc_schedule(void)
 {
+    fih_int fih_rc = FIH_FAILURE;
     AAPCS_DUAL_U32_T ctx_ctrls;
     struct partition_t *p_part_curr, *p_part_next;
     struct context_ctrl_t *p_curr_ctx;
@@ -276,9 +278,9 @@
          * implementation. Change privilege, MPU or other configurations.
          */
         if (p_part_curr->boundary != p_part_next->boundary) {
-            if (tfm_hal_activate_boundary(p_part_next->p_ldinf,
-                                          p_part_next->boundary)
-                                                        != TFM_HAL_SUCCESS) {
+            FIH_CALL(tfm_hal_activate_boundary, fih_rc,
+                     p_part_next->p_ldinf, p_part_next->boundary);
+            if (fih_not_eq(fih_rc, fih_int_encode(TFM_HAL_SUCCESS))) {
                 tfm_core_panic();
             }
         }
diff --git a/secure_fw/spm/ffm/interrupt.c b/secure_fw/spm/ffm/interrupt.c
index a873a4f..802657b 100644
--- a/secure_fw/spm/ffm/interrupt.c
+++ b/secure_fw/spm/ffm/interrupt.c
@@ -37,6 +37,7 @@
     struct partition_t *p_curr_sp;
     uintptr_t sp_base, sp_limit, curr_stack, ctx_stack;
     struct context_ctrl_t flih_ctx_ctrl;
+    fih_int fih_rc = FIH_FAILURE;
 
     /* Come too early before runtime setup, should not happen. */
     if (!CURRENT_THREAD) {
@@ -58,8 +59,8 @@
     }
 
     if (p_owner_sp->boundary != p_curr_sp->boundary) {
-        tfm_hal_activate_boundary(p_owner_sp->p_ldinf,
-                                  p_owner_sp->boundary);
+        FIH_CALL(tfm_hal_activate_boundary, fih_rc,
+                 p_owner_sp->p_ldinf, p_owner_sp->boundary);
     }
 
     /*
@@ -85,13 +86,14 @@
                                 struct context_flih_ret_t *p_ctx_flih_ret)
 {
     struct partition_t *p_prev_sp, *p_owner_sp;
+    fih_int fih_rc = FIH_FAILURE;
 
     p_prev_sp = (struct partition_t *)(p_ctx_flih_ret->state_ctx.r2);
     p_owner_sp = GET_CURRENT_COMPONENT();
 
     if (p_owner_sp->boundary != p_prev_sp->boundary) {
-        tfm_hal_activate_boundary(p_prev_sp->p_ldinf,
-                                  p_prev_sp->boundary);
+        FIH_CALL(tfm_hal_activate_boundary, fih_rc,
+                 p_prev_sp->p_ldinf, p_prev_sp->boundary);
     }
 
     /* Restore current component */
diff --git a/secure_fw/spm/ffm/psa_api.c b/secure_fw/spm/ffm/psa_api.c
index a93b2c9..c0c4aac 100644
--- a/secure_fw/spm/ffm/psa_api.c
+++ b/secure_fw/spm/ffm/psa_api.c
@@ -156,6 +156,7 @@
     int32_t type = (int32_t)(int16_t)((ctrl_param & TYPE_MASK) >> TYPE_OFFSET);
     size_t in_num = (size_t)((ctrl_param & IN_LEN_MASK) >> IN_LEN_OFFSET);
     size_t out_num = (size_t)((ctrl_param & OUT_LEN_MASK) >> OUT_LEN_OFFSET);
+    fih_int fih_rc = FIH_FAILURE;
 
     /* The request type must be zero or positive. */
     if (type < 0) {
@@ -247,9 +248,10 @@
      * if the memory reference for the wrap input vector is invalid or not
      * readable.
      */
-    if (tfm_hal_memory_check(curr_partition->boundary,
-                             (uintptr_t)inptr, in_num * sizeof(psa_invec),
-                             TFM_HAL_ACCESS_READABLE) != PSA_SUCCESS) {
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             curr_partition->boundary, (uintptr_t)inptr,
+             in_num * sizeof(psa_invec), TFM_HAL_ACCESS_READABLE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         return PSA_ERROR_PROGRAMMER_ERROR;
     }
 
@@ -258,8 +260,10 @@
      * actual length later. It is a PROGRAMMER ERROR if the memory reference for
      * the wrap output vector is invalid or not read-write.
      */
-    if (tfm_hal_memory_check(curr_partition->boundary, (uintptr_t)outptr,
-      out_num * sizeof(psa_outvec), TFM_HAL_ACCESS_READWRITE) != PSA_SUCCESS) {
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             curr_partition->boundary, (uintptr_t)outptr,
+             out_num * sizeof(psa_outvec), TFM_HAL_ACCESS_READWRITE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         return PSA_ERROR_PROGRAMMER_ERROR;
     }
 
@@ -275,9 +279,10 @@
      * memory reference was invalid or not readable.
      */
     for (i = 0; i < in_num; i++) {
-        if (tfm_hal_memory_check(curr_partition->boundary,
-                                 (uintptr_t)invecs[i].base, invecs[i].len,
-                                 TFM_HAL_ACCESS_READABLE) != PSA_SUCCESS) {
+        FIH_CALL(tfm_hal_memory_check, fih_rc,
+                 curr_partition->boundary, (uintptr_t)invecs[i].base,
+                 invecs[i].len, TFM_HAL_ACCESS_READABLE);
+        if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
             return PSA_ERROR_PROGRAMMER_ERROR;
         }
     }
@@ -303,9 +308,10 @@
      * payload memory reference was invalid or not read-write.
      */
     for (i = 0; i < out_num; i++) {
-        if (tfm_hal_memory_check(curr_partition->boundary,
-                                 (uintptr_t)outvecs[i].base, outvecs[i].len,
-                                 TFM_HAL_ACCESS_READWRITE) != PSA_SUCCESS) {
+        FIH_CALL(tfm_hal_memory_check, fih_rc,
+                 curr_partition->boundary, (uintptr_t)outvecs[i].base,
+                 outvecs[i].len, TFM_HAL_ACCESS_READWRITE);
+        if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
             return PSA_ERROR_PROGRAMMER_ERROR;
         }
     }
@@ -479,6 +485,7 @@
 {
     struct conn_handle_t *handle = NULL;
     struct partition_t *partition = NULL;
+    fih_int fih_rc = FIH_FAILURE;
 
     /*
      * Only one message could be retrieved every time for psa_get(). It is a
@@ -494,8 +501,10 @@
      * 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_hal_memory_check(partition->boundary, (uintptr_t)msg,
-        sizeof(psa_msg_t), TFM_HAL_ACCESS_READWRITE) != PSA_SUCCESS) {
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             partition->boundary, (uintptr_t)msg,
+             sizeof(psa_msg_t), TFM_HAL_ACCESS_READWRITE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         tfm_core_panic();
     }
 
@@ -536,6 +545,7 @@
     size_t bytes;
     struct conn_handle_t *handle = NULL;
     struct partition_t *curr_partition = GET_CURRENT_COMPONENT();
+    fih_int fih_rc = FIH_FAILURE;
 
     /* It is a fatal error if message handle is invalid */
     handle = spm_get_handle_by_msg_handle(msg_handle);
@@ -580,8 +590,10 @@
      * Copy the client data to the service buffer. It is a fatal error
      * if the memory reference for buffer is invalid or not read-write.
      */
-    if (tfm_hal_memory_check(curr_partition->boundary, (uintptr_t)buffer,
-                         num_bytes, TFM_HAL_ACCESS_READWRITE) != PSA_SUCCESS) {
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             curr_partition->boundary, (uintptr_t)buffer,
+             num_bytes, TFM_HAL_ACCESS_READWRITE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         tfm_core_panic();
     }
 
@@ -663,6 +675,7 @@
 {
     struct conn_handle_t *handle = NULL;
     struct partition_t *curr_partition = GET_CURRENT_COMPONENT();
+    fih_int fih_rc = FIH_FAILURE;
 
     /* It is a fatal error if message handle is invalid */
     handle = spm_get_handle_by_msg_handle(msg_handle);
@@ -711,8 +724,10 @@
      * Copy the service buffer to client outvecs. It is a fatal error
      * if the memory reference for buffer is invalid or not readable.
      */
-    if (tfm_hal_memory_check(curr_partition->boundary,
-       (uintptr_t)buffer, num_bytes, TFM_HAL_ACCESS_READABLE) != PSA_SUCCESS) {
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             curr_partition->boundary, (uintptr_t)buffer,
+             num_bytes, TFM_HAL_ACCESS_READABLE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         tfm_core_panic();
     }
 
@@ -1020,6 +1035,7 @@
 {
     struct conn_handle_t *handle;
     struct partition_t *partition = NULL;
+    fih_int fih_rc = FIH_FAILURE;
 
     /* It is a fatal error if message handle is invalid */
     handle = spm_get_handle_by_msg_handle(msg_handle);
@@ -1078,10 +1094,10 @@
      * It is a fatal error if the memory reference for the wrap input vector is
      * invalid or not readable.
      */
-    if (tfm_hal_memory_check(partition->boundary,
-                             (uintptr_t)handle->invec[invec_idx].base,
-                             handle->invec[invec_idx].len,
-                             TFM_HAL_ACCESS_READABLE) != PSA_SUCCESS) {
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             partition->boundary, (uintptr_t)handle->invec[invec_idx].base,
+             handle->invec[invec_idx].len, TFM_HAL_ACCESS_READABLE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         tfm_core_panic();
     }
 
@@ -1150,6 +1166,7 @@
     struct conn_handle_t *handle;
     uint32_t privileged;
     struct partition_t *partition = NULL;
+    fih_int fih_rc = FIH_FAILURE;
 
     /* It is a fatal error if message handle is invalid */
     handle = spm_get_handle_by_msg_handle(msg_handle);
@@ -1208,10 +1225,10 @@
     /*
      * It is a fatal error if the output vector is invalid or not read-write.
      */
-    if (tfm_hal_memory_check(partition->boundary,
-           (uintptr_t)handle->outvec[outvec_idx].base,
-           handle->outvec[outvec_idx].len,
-           TFM_HAL_ACCESS_READWRITE) != PSA_SUCCESS) {
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             partition->boundary, (uintptr_t)handle->outvec[outvec_idx].base,
+             handle->outvec[outvec_idx].len, TFM_HAL_ACCESS_READWRITE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         tfm_core_panic();
     }
     SET_IOVEC_MAPPED(handle, (outvec_idx + OUTVEC_IDX_BASE));
diff --git a/secure_fw/spm/ffm/tfm_boot_data.c b/secure_fw/spm/ffm/tfm_boot_data.c
index deb88e7..eb6bc8d 100644
--- a/secure_fw/spm/ffm/tfm_boot_data.c
+++ b/secure_fw/spm/ffm/tfm_boot_data.c
@@ -156,6 +156,7 @@
     uint32_t res;
 #else
     struct partition_t *curr_partition = GET_CURRENT_COMPONENT();
+    fih_int fih_rc = FIH_FAILURE;
 #endif
 
 #ifndef TFM_PSA_API
@@ -174,10 +175,10 @@
     }
 #else
 
-    if (tfm_hal_memory_check(curr_partition->boundary,
-                             (uintptr_t)buf_start, buf_size,
-                             TFM_HAL_ACCESS_READWRITE) != PSA_SUCCESS) {
-        /* Not in accessible range, return error */
+    FIH_CALL(tfm_hal_memory_check, fih_rc,
+             curr_partition->boundary, (uintptr_t)buf_start,
+             buf_size, TFM_HAL_ACCESS_READWRITE);
+    if (fih_not_eq(fih_rc, fih_int_encode(PSA_SUCCESS))) {
         args[0] = (uint32_t)TFM_ERROR_INVALID_PARAMETER;
         return;
     }