FIH: Re-enable FIH on AN521

1. Define FIH_RET_TYPE macro to avoid defining a function twice with
   different return types.
2. Rename tfm_fih_implementation to tfm_fih.
3. Change all FIH protected functions into FIH format.
4. Expand stack sizes because SPM needs more stack to call FIH function.
   - S_MSP_STACK_SIZE:           2048 -> 2496
   - TFM_NS_AGENT_TZ_STACK_SIZE: 1024 -> 1256

Signed-off-by: Xinyu Zhang <xinyu.zhang@arm.com>
Change-Id: Ic0136f0232b1e99940fddcbc57e2898516e9d1fc
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
 )