diff --git a/app/os_wrapper_cmsis_rtos_v2.c b/app/os_wrapper_cmsis_rtos_v2.c
index ec785f2..1e188ab 100755
--- a/app/os_wrapper_cmsis_rtos_v2.c
+++ b/app/os_wrapper_cmsis_rtos_v2.c
@@ -8,6 +8,7 @@
 #include "os_wrapper/thread.h"
 #include "os_wrapper/mutex.h"
 #include "os_wrapper/semaphore.h"
+#include "os_wrapper/delay.h"
 
 #include "cmsis_os2.h"
 
@@ -260,3 +261,15 @@
 
     return OS_WRAPPER_ERROR;
 }
+
+int32_t os_wrapper_delay_until(uint32_t ticks)
+{
+    osStatus_t status;
+
+    status = osDelayUntil(ticks);
+    if (status == osOK) {
+        return OS_WRAPPER_SUCCESS;
+    }
+
+    return OS_WRAPPER_ERROR;
+}
diff --git a/ns_interface/os_wrapper/delay.h b/ns_interface/os_wrapper/delay.h
new file mode 100644
index 0000000..9eaf95f
--- /dev/null
+++ b/ns_interface/os_wrapper/delay.h
@@ -0,0 +1,29 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef __OS_WRAPPER_DELAY_H__
+#define __OS_WRAPPER_DELAY_H__
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+#include "common.h"
+
+/**
+ * \brief Wait until an absolute time (specified in kernel ticks) is reached.
+ *
+ * \return \ref OS_WRAPPER_SUCCESS if the time delay is executed, or
+ *         \ref OS_WRAPPER_ERROR in case of error
+ */
+int32_t os_wrapper_delay_until(uint32_t ticks);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif /* __OS_WRAPPER_DELAY_H__ */
diff --git a/test/secure_fw/suites/fpu/fpu_tests_common.c b/test/secure_fw/suites/fpu/fpu_tests_common.c
index 98cb83f..e31ae4e 100644
--- a/test/secure_fw/suites/fpu/fpu_tests_common.c
+++ b/test/secure_fw/suites/fpu/fpu_tests_common.c
@@ -92,8 +92,8 @@
 /**
  * Check whether FP registers are restored correctly.
  * Return:
- *   1 - FP registers are restored correctly
- *   0 - FP registers are not restored correctly
+ *   True - FP registers are restored correctly
+ *   False - FP registers are not restored correctly
  */
 static bool check_fp_restored_client(void)
 {
@@ -170,8 +170,8 @@
 /**
  * Check invalidation of FP registers.
  * Return:
- *   1 - FP registers are invalidated
- *   0 - FP registers are not invalidated
+ *   True - FP registers are invalidated
+ *   False - FP registers are not invalidated
  */
 static bool check_fp_invalidated(void)
 {
diff --git a/test/secure_fw/suites/fpu/fpu_tests_common.h b/test/secure_fw/suites/fpu/fpu_tests_common.h
index 56b2ca1..1787032 100644
--- a/test/secure_fw/suites/fpu/fpu_tests_common.h
+++ b/test/secure_fw/suites/fpu/fpu_tests_common.h
@@ -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
  *
@@ -20,10 +20,22 @@
 #endif
 
 
-#define LOOP_ITERATIONS                         (300U)
+#define LOOP_ITERATIONS             (300U)
+#define LOOPS_S_INT_TEST            (50U)
+#define LOOPS_NS_INT_TEST           (5000000U)
+#define WAIT_S_INT                  (20000U)
 
-#define NR_FP_REG                               (32U)
-#define FP_BUF_SIZE                             (NR_FP_REG * sizeof(uint32_t))
+#define NR_FP_REG                   (32U)
+#define NR_FP_CALLER_REG            (NR_FP_REG / 2)
+#define NR_FP_CALLEE_REG            (NR_FP_REG / 2)
+#define FP_BUF_SIZE                 (NR_FP_REG * sizeof(uint32_t))
+#define FP_CALLER_BUF_SIZE          (NR_FP_CALLER_REG * sizeof(uint32_t))
+#define FP_CALLEE_BUF_SIZE          (NR_FP_CALLEE_REG * sizeof(uint32_t))
+
+#define REL_VALUE_FP_REGS_INVALIDATED        (0xDEADBEEF)
+
+#define S_TIMER_TRIGGERED                    (0x19)
+#define S_TIMER_NOT_TRIGGERED                (0x91)
 
 /**
  * Test FP context protection after psa calls.
diff --git a/test/secure_fw/suites/fpu/non_secure/fpu_ns_interface_testsuite.c b/test/secure_fw/suites/fpu/non_secure/fpu_ns_interface_testsuite.c
index f39fd9a..a72cde6 100644
--- a/test/secure_fw/suites/fpu/non_secure/fpu_ns_interface_testsuite.c
+++ b/test/secure_fw/suites/fpu/non_secure/fpu_ns_interface_testsuite.c
@@ -6,8 +6,18 @@
  */
 
 #include <stdio.h>
+#include <stdbool.h>
+#include <string.h>
 #include "fpu_ns_tests.h"
 #include "../fpu_tests_common.h"
+#include "os_wrapper/delay.h"
+
+static void tfm_fpu_test_fp_protection_secure_interrupt(
+                                                struct test_result_t *ret);
+static void tfm_fpu_test_fp_protection_non_secure_interrupt(
+                                                struct test_result_t *ret);
+static bool check_fp_caller_restored(void);
+static bool check_fp_callee_restored(void);
 
 static struct test_t fpu_ns_tests[] = {
     {
@@ -19,6 +29,17 @@
         &tfm_fpu_test_fp_protection_psa_call, "TFM_NS_FPU_TEST_1002",
         "Test FP context protection after psa calls",
         {TEST_PASSED}
+    },
+    {
+        &tfm_fpu_test_fp_protection_secure_interrupt, "TFM_NS_FPU_TEST_1003",
+        "Test FP context protection in S interrupt after interrupt return",
+        {TEST_PASSED}
+    },
+    {
+        &tfm_fpu_test_fp_protection_non_secure_interrupt,
+        "TFM_NS_FPU_TEST_1004",
+        "Test FP context protection in S thread after NS interrupt",
+        {TEST_PASSED}
     }
 };
 
@@ -31,3 +52,243 @@
     set_testsuite("FPU non-secure interface test (TFM_NS_FPU_TEST_1XXX)",
                   fpu_ns_tests, list_size, p_test_suite);
 }
+
+/**
+ * Change FP registers.
+ */
+__attribute__((naked)) void change_fp_in_ns_thread(void)
+{
+    __asm volatile(
+        "mov       r0, #0xC0000000         \n"
+        "vmov      s0, r0                  \n"
+        "mov       r0, #0xC1000000         \n"
+        "vmov      s1, r0                  \n"
+        "mov       r0, #0xC2000000         \n"
+        "vmov      s2, r0                  \n"
+        "mov       r0, #0xC3000000         \n"
+        "vmov      s3, r0                  \n"
+        "mov       r0, #0xC4000000         \n"
+        "vmov      s4, r0                  \n"
+        "mov       r0, #0xC5000000         \n"
+        "vmov      s5, r0                  \n"
+        "mov       r0, #0xC6000000         \n"
+        "vmov      s6, r0                  \n"
+        "mov       r0, #0xC7000000         \n"
+        "vmov      s7, r0                  \n"
+        "mov       r0, #0xC8000000         \n"
+        "vmov      s8, r0                  \n"
+        "mov       r0, #0xC9000000         \n"
+        "vmov      s9, r0                  \n"
+        "mov       r0, #0xCA000000         \n"
+        "vmov      s10, r0                 \n"
+        "mov       r0, #0xCB000000         \n"
+        "vmov      s11, r0                 \n"
+        "mov       r0, #0xCC000000         \n"
+        "vmov      s12, r0                 \n"
+        "mov       r0, #0xCD000000         \n"
+        "vmov      s13, r0                 \n"
+        "mov       r0, #0xCE000000         \n"
+        "vmov      s14, r0                 \n"
+        "mov       r0, #0xCF000000         \n"
+        "vmov      s15, r0                 \n"
+        "mov       r0, #0xD0000000         \n"
+        "vmov      s16, r0                 \n"
+        "mov       r0, #0xD1000000         \n"
+        "vmov      s17, r0                 \n"
+        "mov       r0, #0xD2000000         \n"
+        "vmov      s18, r0                 \n"
+        "mov       r0, #0xD3000000         \n"
+        "vmov      s19, r0                 \n"
+        "mov       r0, #0xD4000000         \n"
+        "vmov      s20, r0                 \n"
+        "mov       r0, #0xD5000000         \n"
+        "vmov      s21, r0                 \n"
+        "mov       r0, #0xD6000000         \n"
+        "vmov      s22, r0                 \n"
+        "mov       r0, #0xD7000000         \n"
+        "vmov      s23, r0                 \n"
+        "mov       r0, #0xD8000000         \n"
+        "vmov      s24, r0                 \n"
+        "mov       r0, #0xD9000000         \n"
+        "vmov      s25, r0                 \n"
+        "mov       r0, #0xDA000000         \n"
+        "vmov      s26, r0                 \n"
+        "mov       r0, #0xDB000000         \n"
+        "vmov      s27, r0                 \n"
+        "mov       r0, #0xDC000000         \n"
+        "vmov      s28, r0                 \n"
+        "mov       r0, #0xDD000000         \n"
+        "vmov      s29, r0                 \n"
+        "mov       r0, #0xDE000000         \n"
+        "vmov      s30, r0                 \n"
+        "mov       r0, #0xDF000000         \n"
+        "vmov      s31, r0                 \n"
+
+        "bx        lr                      \n"
+    );
+}
+
+/**
+ * Check whether FP caller registers are restored correctly.
+ * Return:
+ *   True - FP caller registers are restored correctly
+ *   False - FP caller registers are not restored correctly
+ */
+static bool check_fp_caller_restored(void)
+{
+    static uint32_t fp_buffer[NR_FP_CALLER_REG] = {0};
+    const uint32_t fp_expect[NR_FP_CALLER_REG] = {
+        0xC0000000, 0xC1000000, 0xC2000000, 0xC3000000,
+        0xC4000000, 0xC5000000, 0xC6000000, 0xC7000000,
+        0xC8000000, 0xC9000000, 0xCA000000, 0xCB000000,
+        0xCC000000, 0xCD000000, 0xCE000000, 0xCF000000
+    };
+
+    __asm volatile(
+        "vstm      %0, {S0-S15}            \n"
+        :
+        :"r"(fp_buffer)
+        :"memory"
+    );
+
+    if (!memcmp(fp_buffer, fp_expect, FP_CALLER_BUF_SIZE)) {
+        return true;
+    }
+
+    return false;
+}
+
+/**
+ * Check whether FP callee registers are restored correctly.
+ * Return:
+ *   True - FP callee registers are restored correctly
+ *   False - FP callee registers are not restored correctly
+ */
+static bool check_fp_callee_restored(void)
+{
+    static uint32_t fp_buffer[NR_FP_CALLEE_REG] = {0};
+    const uint32_t fp_expect[NR_FP_CALLEE_REG] = {
+        0xD0000000, 0xD1000000, 0xD2000000, 0xD3000000,
+        0xD4000000, 0xD5000000, 0xD6000000, 0xD7000000,
+        0xD8000000, 0xD9000000, 0xDA000000, 0xDB000000,
+        0xDC000000, 0xDD000000, 0xDE000000, 0xDF000000
+    };
+
+    __asm volatile(
+        "vstm      %0, {S16-S31}            \n"
+        :
+        :"r"(fp_buffer)
+        :"memory"
+    );
+
+    if (!memcmp(fp_buffer, fp_expect, FP_CALLEE_BUF_SIZE)) {
+        return true;
+    }
+
+    return false;
+}
+
+/**
+ * \brief Test FP context protection in S interrupt. Change FP registers in
+ * non-secure thread first, then change them in S interrupt. After interrupt
+ * return, check FP context protection in non-secure thread.
+ * Expectation: FP registers in S interrupt should not be view in non-secure
+ * thread.
+ */
+static void tfm_fpu_test_fp_protection_secure_interrupt(
+                                                struct test_result_t *ret)
+{
+    psa_handle_t handle;
+    psa_status_t status;
+    uint8_t outvec_data[1] = {0};
+    struct psa_outvec outvecs[1] = {{outvec_data, sizeof(outvec_data[0])}};
+    static uint8_t i;
+
+    ret->val = TEST_FAILED;
+
+    /* Change FP regs */
+    change_fp_in_ns_thread();
+
+    /* Start the timer */
+    handle = psa_connect(TFM_FPU_SERVICE_START_S_TIMER_SID,
+                            TFM_FPU_SERVICE_START_S_TIMER_VERSION);
+    if (!PSA_HANDLE_IS_VALID(handle)) {
+        return;
+    }
+    status = psa_call(handle, PSA_IPC_CALL, NULL, 0, NULL, 0);
+    if (status != PSA_SUCCESS) {
+        return;
+    }
+    psa_close(handle);
+
+    handle = psa_connect(TFM_FPU_SERVICE_CHECK_S_TIMER_TRIGGERED_SID,
+                            TFM_FPU_SERVICE_CHECK_S_TIMER_TRIGGERED_VERSION);
+    if (!PSA_HANDLE_IS_VALID(handle)) {
+        return;
+    }
+    /* Spin here */
+    while (1) {
+        /* Wait S interrupt triggered */
+        os_wrapper_delay_until(WAIT_S_INT);
+
+        status = psa_call(handle, PSA_IPC_CALL, NULL, 0, outvecs, 1);
+        if (status == PSA_SUCCESS) {
+            /* S interrupt triggered */
+            if (outvec_data[0] == S_TIMER_TRIGGERED) {
+                break;
+            } else {
+                i++;
+                if (i > LOOPS_S_INT_TEST) {
+                    TEST_FAIL("Time out: NS thread not interrupted!\r\n");
+                    psa_close(handle);
+                    return;
+                }
+            }
+        } else {
+            TEST_FAIL("Check S interrupt triggered failed!\r\n");
+            return;
+        }
+    }
+
+    psa_close(handle);
+
+    /* FP caller registers should be restored correctly after S interrupt */
+    if (!check_fp_caller_restored()) {
+        return;
+    }
+
+    /* FP callee registers should be restored correctly */
+    if (check_fp_callee_restored()) {
+        ret->val = TEST_PASSED;
+    }
+}
+
+/**
+ * \brief In secure thread, trigger non-secure timer interrupt, check FP
+ *  context protection after NS interrupt.
+ * Expectation: FP register in secure thread should be restored after NS
+ *  interrupt.
+ */
+static void tfm_fpu_test_fp_protection_non_secure_interrupt(
+                                                    struct test_result_t *ret)
+{
+    psa_handle_t handle;
+    psa_status_t status;
+    uint8_t *outvec_data[1] = {0};
+    struct psa_outvec outvecs[1] = {{outvec_data, sizeof(outvec_data[0])}};
+
+    ret->val = TEST_FAILED;
+
+    handle = psa_connect(TFM_FPU_SERVICE_CHECK_NS_INTERRUPT_S_TEST_SID,
+                            TFM_FPU_SERVICE_CHECK_NS_INTERRUPT_S_TEST_VERSION);
+    if (!PSA_HANDLE_IS_VALID(handle)) {
+        return;
+    }
+
+    status = psa_call(handle, PSA_IPC_CALL, NULL, 0, outvecs, 1);
+    if (status == PSA_SUCCESS) {
+        ret->val = TEST_PASSED;
+    }
+
+    psa_close(handle);
+}
diff --git a/test/secure_fw/suites/fpu/service/CMakeLists.txt b/test/secure_fw/suites/fpu/service/CMakeLists.txt
index 05ace67..038d210 100644
--- a/test/secure_fw/suites/fpu/service/CMakeLists.txt
+++ b/test/secure_fw/suites/fpu/service/CMakeLists.txt
@@ -15,7 +15,7 @@
 
 target_sources(tfm_app_rot_partition_fpu_service
     PRIVATE
-        ./tfm_fpu_service_test.c
+        tfm_fpu_service_test.c
 )
 
 # The generated sources
@@ -36,6 +36,7 @@
         .
     PRIVATE
         ${CMAKE_BINARY_DIR}/generated/secure_fw/test_services/tfm_fpu_service
+        ${TFM_TEST_PATH}/secure_fw/suites/fpu
 )
 
 target_include_directories(tfm_partitions
diff --git a/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.c b/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.c
index 91bee8c..206b27a 100644
--- a/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.c
+++ b/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  *
@@ -12,21 +12,16 @@
 #include "tfm_api.h"
 #include "tfm_hal_isolation.h"
 #include "tfm_secure_api.h"
+#include "tfm_memory_utils.h"
+#include "tfm_sp_log.h"
+#include "tfm_plat_test.h"
+#include "device_definition.h"
+#include "fpu_tests_common.h"
 
 /* Define the return status */
 #define FPU_SP_TEST_SUCCESS     (0)
 #define FPU_SP_TEST_FAILED      (-1)
 
-/*
- * Fixme: Temporarily implement abort as infinite loop,
- * will replace it later.
- */
-static void tfm_abort(void)
-{
-    while (1)
-        ;
-}
-
 /**
  * Clear FP registers.
  */
@@ -73,148 +68,32 @@
 /**
  * Check whether FP registers are restored correctly.
  * Return:
- *   1 - FP registers are restored correctly
- *   0 - FP registers are not restored correctly
+ *   True - FP registers are restored correctly
+ *   False - FP registers are not restored correctly
  */
-__attribute__((naked)) static bool check_fp_restored_service(void)
+bool check_fp_restored_service(void)
 {
+    uint32_t fp_buffer[NR_FP_REG] = {0};
+    const uint32_t fp_expect[NR_FP_REG] = {
+        0xE0, 0xE1, 0xE2, 0xE3, 0xE4, 0xE5, 0xE6, 0xE7,
+        0xE8, 0xE9, 0xEA, 0xEB, 0xEC, 0xED, 0xEE, 0xEF,
+        0xF0, 0xF1, 0xF2, 0xF3, 0xF4, 0xF5, 0xF6, 0xF7,
+        0xF8, 0xF9, 0xFA, 0xFB, 0xFC, 0xFD, 0xFE, 0xFF
+     };
+
+    /* Dump FP data from FP registers to buffer */
     __asm volatile(
-        "mov       r3, #0                  \n"
-
-        "vmov      r2, s0                  \n"
-        "cmp       r2, 0x000000E0          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s1                  \n"
-        "cmp       r2, 0x000000E1          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s2                  \n"
-        "cmp       r2, 0x000000E2          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s3                  \n"
-        "cmp       r2, 0x000000E3          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s4                  \n"
-        "cmp       r2, 0x000000E4          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s5                  \n"
-        "cmp       r2, 0x000000E5          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s6                  \n"
-        "cmp       r2, 0x000000E6          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s7                  \n"
-        "cmp       r2, 0x000000E7          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s8                  \n"
-        "cmp       r2, 0x000000E8          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s9                  \n"
-        "cmp       r2, 0x000000E9          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s10                 \n"
-        "cmp       r2, 0x000000EA          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s11                 \n"
-        "cmp       r2, 0x000000EB          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s12                 \n"
-        "cmp       r2, 0x000000EC          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s13                 \n"
-        "cmp       r2, 0x000000ED          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s14                 \n"
-        "cmp       r2, 0x000000EE          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s15                 \n"
-        "cmp       r2, 0x000000EF          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s16                 \n"
-        "cmp       r2, 0x000000F0          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s17                 \n"
-        "cmp       r2, 0x000000F1          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s18                 \n"
-        "cmp       r2, 0x000000F2          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s19                 \n"
-        "cmp       r2, 0x000000F3          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s20                 \n"
-        "cmp       r2, 0x000000F4          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s21                 \n"
-        "cmp       r2, 0x000000F5          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s22                 \n"
-        "cmp       r2, 0x000000F6          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s23                 \n"
-        "cmp       r2, 0x000000F7          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s24                 \n"
-        "cmp       r2, 0x000000F8          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s25                 \n"
-        "cmp       r2, 0x000000F9          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s26                 \n"
-        "cmp       r2, 0x000000FA          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s27                 \n"
-        "cmp       r2, 0x000000FB          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s28                 \n"
-        "cmp       r2, 0x000000FC          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s29                 \n"
-        "cmp       r2, 0x000000FD          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s30                 \n"
-        "cmp       r2, 0x000000FE          \n"
-        "bne       quit                    \n"
-
-        "vmov      r2, s31                 \n"
-        "cmp       r2, 0x000000FF          \n"
-        "bne       quit                    \n"
-
-        "mov       r3, #1                  \n"
-        "quit:                             \n"
-        "mov       r0, r3                  \n"
-
-        "bx        lr                      \n"
+        "vstm      %0, {S0-S31}            \n"
+        :
+        :"r"(fp_buffer)
+        :"memory"
     );
+
+    if (!tfm_memcmp(fp_buffer, fp_expect, FP_BUF_SIZE)) {
+        return true;
+    }
+
+    return false;
 }
 
 /**
@@ -295,39 +174,141 @@
 /**
  * Check whether FP registers is invalidated.
  */
-__attribute__((naked)) static bool is_fp_cleaned(void)
+bool is_fp_cleaned(void)
+{
+    uint32_t fp_buffer[NR_FP_REG] = {0};
+    uint32_t i;
+
+    /* Dump FP data from FP registers to buffer */
+    __asm volatile(
+        "vstm      %0, {S0-S31}            \n"
+        :
+        :"r"(fp_buffer)
+        :"memory"
+    );
+
+    for (i = 0; i < NR_FP_REG; i++) {
+        if (fp_buffer[i] != 0) {
+            return false;
+        }
+    }
+
+    return true;
+}
+
+/**
+ * Change FP registers in secure thread.
+ */
+__attribute__((naked)) void change_fp_in_s_thread(void)
 {
     __asm volatile(
-        "mov       r3, #1                  \n"
-        "mov       r2, #0                  \n"
-        "vadd.f32  s2, s1, s0              \n"
-        "vadd.f32  s4, s3, s2              \n"
-        "vadd.f32  s6, s5, s4              \n"
-        "vadd.f32  s8, s7, s6              \n"
-        "vadd.f32  s10, s9, s8             \n"
-        "vadd.f32  s12, s11, s10           \n"
-        "vadd.f32  s14, s13, s12           \n"
-        "vadd.f32  s16, s15, s14           \n"
-        "vadd.f32  s18, s17, s16           \n"
-        "vadd.f32  s20, s19, s18           \n"
-        "vadd.f32  s22, s21, s20           \n"
-        "vadd.f32  s24, s23, s22           \n"
-        "vadd.f32  s26, s25, s24           \n"
-        "vadd.f32  s28, s27, s26           \n"
-        "vadd.f32  s30, s29, s28           \n"
-        "vadd.f32  s31, s29, s30           \n"
-        "vcmp.f32  s31, #0.0               \n"
-        "vmrs      APSR_nzcv, fpscr        \n"
-        "beq       cleaned                 \n"
-        "mov       r3, r2                  \n"
-        "cleaned:                          \n"
-        "mov       r0, r3                  \n"
+        "push      {r4, lr}                    \n"
 
-        "bx       lr                      \n"
+        "mov       r0, #0xB0000000         \n"
+        "vmov      s0, r0                  \n"
+        "mov       r0, #0xB1000000         \n"
+        "vmov      s1, r0                  \n"
+        "mov       r0, #0xB2000000         \n"
+        "vmov      s2, r0                  \n"
+        "mov       r0, #0xB3000000         \n"
+        "vmov      s3, r0                  \n"
+        "mov       r0, #0xB4000000         \n"
+        "vmov      s4, r0                  \n"
+        "mov       r0, #0xB5000000         \n"
+        "vmov      s5, r0                  \n"
+        "mov       r0, #0xB6000000         \n"
+        "vmov      s6, r0                  \n"
+        "mov       r0, #0xB7000000         \n"
+        "vmov      s7, r0                  \n"
+        "mov       r0, #0xB8000000         \n"
+        "vmov      s8, r0                  \n"
+        "mov       r0, #0xB9000000         \n"
+        "vmov      s9, r0                  \n"
+        "mov       r0, #0xBA000000         \n"
+        "vmov      s10, r0                 \n"
+        "mov       r0, #0xBB000000         \n"
+        "vmov      s11, r0                 \n"
+        "mov       r0, #0xBC000000         \n"
+        "vmov      s12, r0                 \n"
+        "mov       r0, #0xBD000000         \n"
+        "vmov      s13, r0                 \n"
+        "mov       r0, #0xBE000000         \n"
+        "vmov      s14, r0                 \n"
+        "mov       r0, #0xBF000000         \n"
+        "vmov      s15, r0                 \n"
+        "mov       r0, #0xC0000000         \n"
+        "vmov      s16, r0                 \n"
+        "mov       r0, #0xC1000000         \n"
+        "vmov      s17, r0                 \n"
+        "mov       r0, #0xC2000000         \n"
+        "vmov      s18, r0                 \n"
+        "mov       r0, #0xC3000000         \n"
+        "vmov      s19, r0                 \n"
+        "mov       r0, #0xC4000000         \n"
+        "vmov      s20, r0                 \n"
+        "mov       r0, #0xC5000000         \n"
+        "vmov      s21, r0                 \n"
+        "mov       r0, #0xC6000000         \n"
+        "vmov      s22, r0                 \n"
+        "mov       r0, #0xC7000000         \n"
+        "vmov      s23, r0                 \n"
+        "mov       r0, #0xC8000000         \n"
+        "vmov      s24, r0                 \n"
+        "mov       r0, #0xC9000000         \n"
+        "vmov      s25, r0                 \n"
+        "mov       r0, #0xCA000000         \n"
+        "vmov      s26, r0                 \n"
+        "mov       r0, #0xCB000000         \n"
+        "vmov      s27, r0                 \n"
+        "mov       r0, #0xCC000000         \n"
+        "vmov      s28, r0                 \n"
+        "mov       r0, #0xCD000000         \n"
+        "vmov      s29, r0                 \n"
+        "mov       r0, #0xCE000000         \n"
+        "vmov      s30, r0                 \n"
+        "mov       r0, #0xCF000000         \n"
+        "vmov      s31, r0                 \n"
+
+        "pop       {r4, pc}                \n"
     );
 }
 
 /**
+ * Check whether FP registers are restored correctly.
+ * Return:
+ *   True - FP registers are restored correctly
+ *   False - FP registers are not restored correctly
+ */
+bool check_fp_restored_s(void)
+{
+    uint32_t fp_buffer[NR_FP_REG] = {0};
+    const uint32_t fp_expect[NR_FP_REG] = {
+        0xB0000000, 0xB1000000, 0xB2000000, 0xB3000000,
+        0xB4000000, 0xB5000000, 0xB6000000, 0xB7000000,
+        0xB8000000, 0xB9000000, 0xBA000000, 0xBB000000,
+        0xBC000000, 0xBD000000, 0xBE000000, 0xBF000000,
+        0xC0000000, 0xC1000000, 0xC2000000, 0xC3000000,
+        0xC4000000, 0xC5000000, 0xC6000000, 0xC7000000,
+        0xC8000000, 0xC9000000, 0xCA000000, 0xCB000000,
+        0xCC000000, 0xCD000000, 0xCE000000, 0xCF000000
+    };
+
+    /* Dump FP data from FP registers to buffer */
+    __asm volatile(
+        "vstm      %0, {S0-S31}            \n"
+        :
+        :"r"(fp_buffer)
+        :"memory"
+    );
+
+    if (!tfm_memcmp(fp_buffer, fp_expect, FP_BUF_SIZE)) {
+        return true;
+    }
+
+    return false;
+}
+
+/**
  * Service handler for clear FP register.
  */
 static void fpu_service_clear_fp_register(void)
@@ -353,8 +334,7 @@
         psa_reply(msg.handle, PSA_SUCCESS);
         break;
     default:
-        /* Should not come here */
-        tfm_abort();
+        psa_panic();
         break;
     }
 }
@@ -383,8 +363,129 @@
         psa_reply(msg.handle, PSA_SUCCESS);
         break;
     default:
-        /* Should not come here */
-        tfm_abort();
+        psa_panic();
+        break;
+    }
+}
+
+/**
+ * Start S timer interrupt.
+ * Expectation: S timer should be started.
+ */
+static void fpu_client_start_secure_timer(void)
+{
+    psa_msg_t msg;
+    psa_status_t r;
+
+    r = psa_get(TFM_FPU_SERVICE_START_S_TIMER_SIGNAL, &msg);
+    switch (msg.type) {
+    case PSA_IPC_CONNECT:
+    case PSA_IPC_DISCONNECT:
+        psa_reply(msg.handle, PSA_SUCCESS);
+        break;
+    case PSA_IPC_CALL:
+        /* Start the timer */
+        tfm_plat_test_secure_timer_start();
+        psa_reply(msg.handle, PSA_SUCCESS);
+        break;
+    default:
+        psa_panic();
+        break;
+    }
+}
+
+/**
+ * Read S timer reload value.
+ * Expectation: S timer reload value should be read.
+ */
+static void fpu_client_check_secure_timer_triggered(void)
+{
+    psa_msg_t msg;
+    psa_status_t r;
+    int val;
+
+    r = psa_get(TFM_FPU_SERVICE_CHECK_S_TIMER_TRIGGERED_SIGNAL, &msg);
+    switch (msg.type) {
+    case PSA_IPC_CONNECT:
+    case PSA_IPC_DISCONNECT:
+        psa_reply(msg.handle, PSA_SUCCESS);
+        break;
+    case PSA_IPC_CALL:
+        if (msg.out_size[0] != 0) {
+            /* Read the timer reload value */
+            if (tfm_plat_test_secure_timer_get_reload_value()
+                            == REL_VALUE_FP_REGS_INVALIDATED) {
+                val = S_TIMER_TRIGGERED;
+            } else {
+                val = S_TIMER_NOT_TRIGGERED;
+            }
+            psa_write(msg.handle, 0, &val, 1);
+            r = PSA_SUCCESS;
+        } else {
+            r = PSA_ERROR_PROGRAMMER_ERROR;
+        }
+        psa_reply(msg.handle, r);
+        break;
+    default:
+        psa_panic();
+        break;
+    }
+}
+
+/**
+ * Test FP context protection after NS interrupt.
+ * Expectation: FP register in secure thread should be restored after NS
+ * interrupt.
+ */
+int fpu_client_non_secure_interrupt_secure_test(void)
+{
+    psa_msg_t msg;
+    psa_status_t r;
+    static uint32_t i;
+
+    r = psa_get(TFM_FPU_SERVICE_CHECK_NS_INTERRUPT_S_TEST_SIGNAL, &msg);
+    switch (msg.type) {
+    case PSA_IPC_CONNECT:
+    case PSA_IPC_DISCONNECT:
+        psa_reply(msg.handle, PSA_SUCCESS);
+        break;
+    case PSA_IPC_CALL:
+        /* Change FP regs */
+        change_fp_in_s_thread();
+        /* Start the timer */
+        tfm_plat_test_non_secure_timer_start();
+        LOG_DBGFMT("Wait for NS timer interrupt!\r\n");
+        /* Spin here */
+        while (1) {
+            /* NS interrupt triggered */
+            if (tfm_plat_test_non_secure_timer_get_reload_value()
+                                == REL_VALUE_FP_REGS_INVALIDATED) {
+                LOG_DBGFMT("S thread interrupted by NS timer interrupt!\r\n");
+                break;
+            } else {
+                i++;
+                if (i > LOOPS_NS_INT_TEST) {
+                    LOG_DBGFMT("Time out: S thread is not interrupted!\r\n");
+                    break;
+                }
+            }
+        }
+        if (i > LOOPS_NS_INT_TEST) {
+            /* Error for time out */
+            r = PSA_ERROR_GENERIC_ERROR;
+        } else {
+            /* FP register should be restored after NS interrupt. */
+            if (check_fp_restored_s()) {
+                r = PSA_SUCCESS;
+            } else {
+                r = PSA_ERROR_GENERIC_ERROR;
+            }
+        }
+        /* Reply with status */
+        psa_reply(msg.handle, r);
+        break;
+    default:
+        psa_panic();
         break;
     }
 }
@@ -403,9 +504,14 @@
             fpu_service_clear_fp_register();
         } else if (signals & TFM_FPU_SERVICE_CHECK_FP_REGISTER_SIGNAL) {
             fpu_service_check_fp_register();
+        } else if (signals & TFM_FPU_SERVICE_START_S_TIMER_SIGNAL) {
+            fpu_client_start_secure_timer();
+        } else if (signals & TFM_FPU_SERVICE_CHECK_S_TIMER_TRIGGERED_SIGNAL) {
+            fpu_client_check_secure_timer_triggered();
+        }  else if (signals & TFM_FPU_SERVICE_CHECK_NS_INTERRUPT_S_TEST_SIGNAL) {
+            fpu_client_non_secure_interrupt_secure_test();
         } else {
-            /* Should not come here */
-            tfm_abort();
+            psa_panic();
         }
     }
 }
diff --git a/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.yaml b/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.yaml
index 4976446..e826b01 100644
--- a/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.yaml
+++ b/test/secure_fw/suites/fpu/service/tfm_fpu_service_test.yaml
@@ -10,14 +10,16 @@
   "name": "TFM_SP_FPU_SERVICE_TEST",
   "type": "APPLICATION-ROT",
   "priority": "NORMAL",
+  "model": "IPC",
   "entry_point": "fpu_service_test_main",
-  "stack_size": "0x0220",
+  "stack_size": "0x0400",
   "secure_functions": [
   ],
   "services" : [
     {
       "name": "TFM_FPU_SERVICE_CLEAR_FP_REGISTER",
       "sid": "0x0000F090",
+      "connection_based": true,
       "non_secure_clients": true,
       "version": 1,
       "version_policy": "STRICT"
@@ -25,9 +27,44 @@
     {
       "name": "TFM_FPU_SERVICE_CHECK_FP_REGISTER",
       "sid": "0x0000F091",
+      "connection_based": true,
       "non_secure_clients": true,
       "version": 1,
       "version_policy": "STRICT"
+    },
+    {
+      "name": "TFM_FPU_SERVICE_START_S_TIMER",
+      "sid": "0x0000F092",
+      "connection_based": true,
+      "non_secure_clients": true,
+      "version": 1,
+      "version_policy": "STRICT"
+    },
+    {
+      "name": "TFM_FPU_SERVICE_CHECK_S_TIMER_TRIGGERED",
+      "sid": "0x0000F093",
+      "connection_based": true,
+      "non_secure_clients": true,
+      "version": 1,
+      "version_policy": "STRICT"
+    },
+    {
+      "name": "TFM_FPU_SERVICE_CHECK_NS_INTERRUPT_S_TEST",
+      "sid": "0x0000F094",
+      "connection_based": true,
+      "non_secure_clients": true,
+      "version": 1,
+      "version_policy": "STRICT"
+    }
+  ],
+  "mmio_regions": [
+    {
+      "name": "TFM_PERIPHERAL_TIMER0",
+      "permission": "READ-WRITE"
+    },
+    {
+      "name": "TFM_PERIPHERAL_TIMER1",
+      "permission": "READ-WRITE"
     }
   ]
 }