diff --git a/platform/ext/common/core_ext.h b/platform/ext/common/core_ext.h
new file mode 100644
index 0000000..097c7e7
--- /dev/null
+++ b/platform/ext/common/core_ext.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef __CORE_EXT_H__
+#define __CORE_EXT_H__
+
+/* Coprocessor Power Control Register Definitions */
+#define SCnSCB_CPPWR_SUS11_Pos              23U                                        /*!< CPPWR: SUS11 Position */
+#define SCnSCB_CPPWR_SUS11_Msk              (1UL << SCnSCB_CPPWR_SUS11_Pos)            /*!< CPPWR: SUS11 Mask */
+
+#define SCnSCB_CPPWR_SU11_Pos               22U                                        /*!< CPPWR: SU11 Position */
+#define SCnSCB_CPPWR_SU11_Msk               (1UL << SCnSCB_CPPWR_SU11_Pos)             /*!< CPPWR: SU11 Mask */
+
+#define SCnSCB_CPPWR_SUS10_Pos              21U                                        /*!< CPPWR: SUS10 Position */
+#define SCnSCB_CPPWR_SUS10_Msk              (1UL << SCnSCB_CPPWR_SUS10_Pos)            /*!< CPPWR: SUS10 Mask */
+
+#define SCnSCB_CPPWR_SU10_Pos               20U                                        /*!< CPPWR: SU10 Position */
+#define SCnSCB_CPPWR_SU10_Msk               (1UL << SCnSCB_CPPWR_SU10_Pos)             /*!< CPPWR: SU10 Mask */
+
+#endif /* __CORE_EXT_H__ */
diff --git a/secure_fw/spm/cmsis_psa/arch/tfm_arch_v8m_main.c b/secure_fw/spm/cmsis_psa/arch/tfm_arch_v8m_main.c
index 444801f..9d97d2b 100644
--- a/secure_fw/spm/cmsis_psa/arch/tfm_arch_v8m_main.c
+++ b/secure_fw/spm/cmsis_psa/arch/tfm_arch_v8m_main.c
@@ -18,6 +18,9 @@
 #include "tfm_secure_api.h"
 #include "tfm_svcalls.h"
 #include "utilities.h"
+#if defined(__FPU_USED) && (__FPU_USED == 1U) && (CONFIG_TFM_SPE_FP >= 1)
+#include "core_ext.h"
+#endif
 
 #if !defined(__ARM_ARCH_8M_MAIN__) && !defined(__ARM_ARCH_8_1M_MAIN__)
 #error "Unsupported ARM Architecture."
@@ -263,25 +266,41 @@
      * latency when the FPU is not used by the SPE.
      */
 #if defined(__FPU_USED) && (__FPU_USED == 1U)
+/* For secure uses FPU only */
+#if (CONFIG_TFM_SPE_FP >= 1)
+#ifdef __GNUC__
     /* Enable Secure privileged and unprivilged access to the FP Extension */
     SCB->CPACR |= (3U << 10U*2U)     /* enable CP10 full access */
                   | (3U << 11U*2U);  /* enable CP11 full access */
+#endif
 
-    /* If the SPE will ever use the floating-point registers for sensitive data,
-     * then FPCCR.TS, FPCCR.CLRONRET and FPCCR.CLRONRETS must be set at
-     * initialisation and not changed again afterwards.
+#ifdef CONFIG_TFM_LAZY_STACKING_SPE
+    /* Enable lazy stacking */
+    FPU->FPCCR |= FPU_FPCCR_LSPEN_Msk;
+#else
+    /* Disable lazy stacking */
+    FPU->FPCCR &= ~FPU_FPCCR_LSPEN_Msk;
+#endif
+    /* If the SPE will ever use the floating-point registers for sensitive
+     * data, then FPCCR.ASPEN, FPCCR.TS, FPCCR.CLRONRET and FPCCR.CLRONRETS
+     * must be set at initialisation and not changed again afterwards.
      */
-    FPU->FPCCR |= FPU_FPCCR_TS_Msk
+    FPU->FPCCR |= FPU_FPCCR_ASPEN_Msk
+                  | FPU_FPCCR_TS_Msk
                   | FPU_FPCCR_CLRONRET_Msk
                   | FPU_FPCCR_CLRONRETS_Msk;
-#endif
 
-    /* Permit Non-secure access to the Floating-point Extension.
-     * Note: It is still necessary to set CPACR_NS to enable the FP Extension in
-     * the NSPE. This configuration is left to NS privileged software.
+    /* If FPU is used by secure only, prevent non-secure from modifying FPU’s
+     * power setting.
      */
-    SCB->NSACR |= SCB_NSACR_CP10_Msk | SCB_NSACR_CP11_Msk;
-#endif
+    SCnSCB->CPPWR |= SCnSCB_CPPWR_SUS11_Msk | SCnSCB_CPPWR_SUS10_Msk;
+
+    /* Disable Non-secure access to the Floating-point Extension.
+     */
+    SCB->NSACR &= ~(SCB_NSACR_CP10_Msk | SCB_NSACR_CP11_Msk);
+#endif /* CONFIG_TFM_SPE_FP >= 1 */
+#endif /* __FPU_USED */
+#endif /* __FPU_PRESENT */
 
 #if defined(__ARM_ARCH_8_1M_MAIN__)
     SCB->CCR |= SCB_CCR_TRD_Msk;
@@ -298,3 +317,46 @@
                    "bx   lr                  \n"
                   );
 }
+
+#if (CONFIG_TFM_SPE_FP >= 1)
+__attribute__((naked, noinline)) void tfm_arch_clear_fp_data(void)
+{
+    __ASM volatile(
+                    "eor  r0, r0, r0         \n"
+                    "vmov s0, r0             \n"
+                    "vmov s1, r0             \n"
+                    "vmov s2, r0             \n"
+                    "vmov s3, r0             \n"
+                    "vmov s4, r0             \n"
+                    "vmov s5, r0             \n"
+                    "vmov s6, r0             \n"
+                    "vmov s7, r0             \n"
+                    "vmov s8, r0             \n"
+                    "vmov s9, r0             \n"
+                    "vmov s10, r0            \n"
+                    "vmov s11, r0            \n"
+                    "vmov s12, r0            \n"
+                    "vmov s13, r0            \n"
+                    "vmov s14, r0            \n"
+                    "vmov s15, r0            \n"
+                    "vmov s16, r0            \n"
+                    "vmov s17, r0            \n"
+                    "vmov s18, r0            \n"
+                    "vmov s19, r0            \n"
+                    "vmov s20, r0            \n"
+                    "vmov s21, r0            \n"
+                    "vmov s22, r0            \n"
+                    "vmov s23, r0            \n"
+                    "vmov s24, r0            \n"
+                    "vmov s25, r0            \n"
+                    "vmov s26, r0            \n"
+                    "vmov s27, r0            \n"
+                    "vmov s28, r0            \n"
+                    "vmov s29, r0            \n"
+                    "vmov s30, r0            \n"
+                    "vmov s31, r0            \n"
+                    "vmsr fpscr, r0          \n"
+                    "bx   lr                 \n"
+                  );
+}
+#endif
diff --git a/secure_fw/spm/cmsis_psa/main.c b/secure_fw/spm/cmsis_psa/main.c
index 34959cb..d586b3c 100644
--- a/secure_fw/spm/cmsis_psa/main.c
+++ b/secure_fw/spm/cmsis_psa/main.c
@@ -146,6 +146,12 @@
      */
     tfm_arch_set_secure_exception_priorities();
 
+#if (CONFIG_TFM_SPE_FP >= 1)
+    tfm_arch_clear_fp_data();
+#endif
+
+    tfm_arch_clear_fp_status();
+
     /* Move to handler mode for further SPM initialization. */
     tfm_core_handler_mode();
 
diff --git a/secure_fw/spm/cmsis_psa/spm_ipc.c b/secure_fw/spm/cmsis_psa/spm_ipc.c
index f9ec628..8198389 100755
--- a/secure_fw/spm/cmsis_psa/spm_ipc.c
+++ b/secure_fw/spm/cmsis_psa/spm_ipc.c
@@ -697,6 +697,7 @@
                 tfm_core_panic();
             }
         }
+        ARCH_FLUSH_FP_CONTEXT();
     }
 
     /*
diff --git a/secure_fw/spm/cmsis_psa/tfm_core_svcalls_ipc.c b/secure_fw/spm/cmsis_psa/tfm_core_svcalls_ipc.c
index 1a08bb9..1ac8199 100644
--- a/secure_fw/spm/cmsis_psa/tfm_core_svcalls_ipc.c
+++ b/secure_fw/spm/cmsis_psa/tfm_core_svcalls_ipc.c
@@ -220,7 +220,6 @@
 
     switch (svc_number) {
     case TFM_SVC_SPM_INIT:
-        tfm_arch_clear_fp_status();
         exc_return = tfm_spm_init();
         /* The following call does not return */
         tfm_arch_free_msp_and_exc_ret(
diff --git a/secure_fw/spm/include/tfm_arch.h b/secure_fw/spm/include/tfm_arch.h
index a754d5f..96a0daf 100644
--- a/secure_fw/spm/include/tfm_arch.h
+++ b/secure_fw/spm/include/tfm_arch.h
@@ -118,6 +118,12 @@
     __ISB();
 }
 
+#if (CONFIG_TFM_SPE_FP >= 1) && CONFIG_TFM_LAZY_STACKING_SPE
+#define ARCH_FLUSH_FP_CONTEXT()  __asm volatile("vmov  s0, s0 \n":::"memory")
+#else
+#define ARCH_FLUSH_FP_CONTEXT()
+#endif
+
 /* Set secure exceptions priority. */
 void tfm_arch_set_secure_exception_priorities(void);
 
@@ -127,6 +133,13 @@
 /* Clear float point status. */
 void tfm_arch_clear_fp_status(void);
 
+#if (CONFIG_TFM_SPE_FP >= 1)
+/*
+ * Clear float point data.
+ */
+void tfm_arch_clear_fp_data(void);
+#endif
+
 /*
  * This function is called after SPM has initialized.
  * It frees the stack used by SPM initialization and do Exception Return.