test(tftf): test FPU state registers context is preserved in RL/SE/NS

Test that FPU/SIMD state are preserved during a randomly context switch
between secure/non-secure/realm(R-EL1)worlds.
FPU/SIMD state consist of the 32 SIMD vectors, FPCR and FPSR registers,
the test runs for 1000 iterations with random combination of:
SECURE_FILL_FPU, SECURE_READ_FPU, REALM_FILL_FPU, REALM_READ_FPU,
NONSECURE_FILL_FPU, NONSECURE_READ_FPU commands,to test all possible
situations of synchronous context switch between worlds, while the
content of those registers is being used.

Signed-off-by: Nabil Kahlouche <nabil.kahlouche@arm.com>
Signed-off-by: Shruti Gupta <shruti.gupta@arm.com>
Change-Id: I6da5fd334777000111924bb1239b77123a3dcea6
diff --git a/include/lib/extensions/fpu.h b/include/lib/extensions/fpu.h
index c803081..a4c4b45 100644
--- a/include/lib/extensions/fpu.h
+++ b/include/lib/extensions/fpu.h
@@ -18,6 +18,7 @@
 
 #ifndef __ASSEMBLER__
 
+#include <stdbool.h>
 #include <stdint.h>
 
 typedef struct fpu_reg_state {
diff --git a/include/runtime_services/host_realm_managment/host_shared_data.h b/include/runtime_services/host_realm_managment/host_shared_data.h
index ca379e2..7720334 100644
--- a/include/runtime_services/host_realm_managment/host_shared_data.h
+++ b/include/runtime_services/host_realm_managment/host_shared_data.h
@@ -43,7 +43,9 @@
 	REALM_PMU_CYCLE,
 	REALM_PMU_EVENT,
 	REALM_PMU_PRESERVE,
-	REALM_PMU_INTERRUPT
+	REALM_PMU_INTERRUPT,
+	REALM_REQ_FPU_FILL_CMD,
+	REALM_REQ_FPU_CMP_CMD
 };
 
 /*
diff --git a/realm/realm.mk b/realm/realm.mk
index b033627..923a687 100644
--- a/realm/realm.mk
+++ b/realm/realm.mk
@@ -12,6 +12,7 @@
 	-Iinclude/common						\
 	-Iinclude/common/${ARCH}					\
 	-Iinclude/lib							\
+	-Iinclude/lib/extensions					\
 	-Iinclude/lib/${ARCH}						\
 	-Iinclude/lib/utils						\
 	-Iinclude/lib/xlat_tables					\
@@ -39,7 +40,8 @@
 	lib/smc/${ARCH}/smc.c						\
 	lib/exceptions/${ARCH}/sync.c					\
 	lib/locks/${ARCH}/spinlock.S					\
-	lib/delay/delay.c
+	lib/delay/delay.c						\
+	lib/extensions/fpu/fpu.c
 
 # TODO: Remove dependency on TFTF files.
 REALM_SOURCES	+=							\
diff --git a/realm/realm_payload_main.c b/realm/realm_payload_main.c
index 414c329..c6665b4 100644
--- a/realm/realm_payload_main.c
+++ b/realm/realm_payload_main.c
@@ -8,6 +8,7 @@
 #include <stdio.h>
 
 #include <debug.h>
+#include <fpu.h>
 #include <host_realm_helper.h>
 #include <host_shared_data.h>
 #include "realm_def.h"
@@ -15,6 +16,7 @@
 #include <realm_tests.h>
 #include <tftf_lib.h>
 
+static fpu_reg_state_t fpu_temp_rl;
 /*
  * This function reads sleep time in ms from shared buffer and spins PE
  * in a loop for that time period.
@@ -85,6 +87,13 @@
 		case REALM_PMU_INTERRUPT:
 			test_succeed = test_pmuv3_overflow_interrupt();
 			break;
+		case REALM_REQ_FPU_FILL_CMD:
+			fpu_state_fill_regs_and_template(&fpu_temp_rl);
+			test_succeed = true;
+			break;
+		case REALM_REQ_FPU_CMP_CMD:
+			test_succeed = fpu_state_compare_template(&fpu_temp_rl);
+			break;
 		default:
 			realm_printf("%s() invalid cmd %u\n", __func__, cmd);
 			break;
diff --git a/tftf/tests/runtime_services/realm_payload/host_realm_spm.c b/tftf/tests/runtime_services/realm_payload/host_realm_spm.c
index de5413f..d2b2a14 100644
--- a/tftf/tests/runtime_services/realm_payload/host_realm_spm.c
+++ b/tftf/tests/runtime_services/realm_payload/host_realm_spm.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2022, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2023, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <cactus_test_cmds.h>
 #include <ffa_endpoints.h>
 #include <ffa_helpers.h>
+#include <fpu.h>
 #include <host_realm_helper.h>
 #include <host_realm_mem_layout.h>
 #include <host_shared_data.h>
@@ -20,19 +21,27 @@
 static const struct ffa_uuid expected_sp_uuids[] = { {PRIMARY_UUID} };
 static struct mailbox_buffers mb;
 static bool secure_mailbox_initialised;
+static fpu_reg_state_t fpu_temp_ns;
+
+typedef enum test_rl_sec_fp_cmd {
+	CMD_SIMD_NS_FILL = 0U,
+	CMD_SIMD_NS_CMP,
+	CMD_SIMD_RL_FILL,
+	CMD_SIMD_RL_CMP,
+	CMD_MAX_THREE_WORLD,
+	CMD_SIMD_SEC_FILL,
+	CMD_SIMD_SEC_CMP,
+	CMD_MAX_COUNT
+} realm_test_cmd_t;
 
 /*
  * This function helps to Initialise secure_mailbox, creates realm payload and
  * shared memory to be used between Host and Realm.
  * Skip test if RME is not supported or not the right RMM version is begin used
  */
-static test_result_t init_test(void)
+static test_result_t init_sp(void)
 {
-	u_register_t retrmm;
-
-
 	/* Verify that FFA is there and that it has the correct version. */
-	SKIP_TEST_IF_AARCH32();
 	SKIP_TEST_IF_FFA_VERSION_LESS_THAN(1, 1);
 
 	if (!secure_mailbox_initialised) {
@@ -40,17 +49,21 @@
 		CHECK_SPMC_TESTING_SETUP(1, 1, expected_sp_uuids);
 		secure_mailbox_initialised = true;
 	}
+	return TEST_RESULT_SUCCESS;
+}
+
+static test_result_t init_realm(void)
+{
+	u_register_t retrmm;
 
 	if (get_armv9_2_feat_rme_support() == 0U) {
 		return TEST_RESULT_SKIPPED;
 	}
 
 	retrmm = host_rmi_version();
-	VERBOSE("RMM version is: %lu.%lu\n",
-			RMI_ABI_VERSION_GET_MAJOR(retrmm),
-			RMI_ABI_VERSION_GET_MINOR(retrmm));
+
 	/*
-	 * Skip the test if RMM is TRP, TRP version is always null.
+	 * Skip test if RMM is TRP, TRP version is always null.
 	 */
 	if (retrmm == 0UL) {
 		return TEST_RESULT_SKIPPED;
@@ -87,6 +100,59 @@
 	return false;
 }
 
+/* Send request to SP to fill FPU/SIMD regs with secure template values */
+static bool fpu_fill_sec(void)
+{
+	struct ffa_value ret = cactus_req_simd_fill_send_cmd(SENDER, RECEIVER);
+
+	if (!is_ffa_direct_response(ret)) {
+		ERROR("%s failed %d\n", __func__, __LINE__);
+		return false;
+	}
+	if (cactus_get_response(ret) == CACTUS_ERROR) {
+		ERROR("%s failed %d\n", __func__, __LINE__);
+		return false;
+	}
+	return true;
+}
+
+/* Send request to SP to compare FPU/SIMD regs with secure template values */
+static bool fpu_cmp_sec(void)
+{
+	struct ffa_value ret = cactus_req_simd_compare_send_cmd(SENDER, RECEIVER);
+
+	if (!is_ffa_direct_response(ret)) {
+		ERROR("%s failed %d\n", __func__, __LINE__);
+		return false;
+	}
+	if (cactus_get_response(ret) == CACTUS_ERROR) {
+		ERROR("%s failed %d\n", __func__, __LINE__);
+		return false;
+	}
+	return true;
+}
+
+
+/* Send request to Realm to fill FPU/SIMD regs with realm template values */
+static bool fpu_fill_rl(void)
+{
+	if (!host_enter_realm_execute(REALM_REQ_FPU_FILL_CMD, NULL, RMI_EXIT_HOST_CALL)) {
+		ERROR("%s failed %d\n", __func__, __LINE__);
+		return false;
+	}
+	return true;
+}
+
+/* Send request to Realm to compare FPU/SIMD regs with previous realm template values */
+static bool fpu_cmp_rl(void)
+{
+	if (!host_enter_realm_execute(REALM_REQ_FPU_FILL_CMD, NULL, RMI_EXIT_HOST_CALL)) {
+		ERROR("%s failed %d\n", __func__, __LINE__);
+		return false;
+	}
+	return true;
+}
+
 /*
  * @Test_Aim@ Test secure interrupt handling while Secure Partition is in waiting
  * state and Realm world runs a busy loop at R-EL1.
@@ -134,7 +200,12 @@
 	struct ffa_value ret_values;
 	test_result_t res;
 
-	res = init_test();
+	res = init_sp();
+	if (res != TEST_RESULT_SUCCESS) {
+		return res;
+	}
+
+	res = init_realm();
 	if (res != TEST_RESULT_SUCCESS) {
 		return res;
 	}
@@ -197,3 +268,102 @@
 
 	return TEST_RESULT_SUCCESS;
 }
+
+/*
+ * Test that FPU/SIMD state are preserved during a randomly context switch
+ * between secure/non-secure/realm(R-EL1)worlds.
+ * FPU/SIMD state consist of the 32 SIMD vectors, FPCR and FPSR registers,
+ * the test runs for 1000 iterations with random combination of:
+ * SECURE_FILL_FPU, SECURE_READ_FPU, REALM_FILL_FPU, REALM_READ_FPU,
+ * NONSECURE_FILL_FPU, NONSECURE_READ_FPU commands,to test all possible situations
+ * of synchronous context switch between worlds, while the content of those registers
+ * is being used.
+ */
+test_result_t host_realm_fpu_access_in_rl_ns_se(void)
+{
+	int cmd = -1, old_cmd  = -1, cmd_max;
+	test_result_t res;
+
+	res = init_sp();
+	if (res != TEST_RESULT_SUCCESS) {
+		cmd_max = CMD_MAX_THREE_WORLD;
+	} else {
+		cmd_max = CMD_MAX_COUNT;
+		if (!fpu_fill_sec()) {
+			ERROR("fpu_fill_sec error\n");
+			return TEST_RESULT_FAIL;
+		}
+	}
+
+	res = init_realm();
+	if (res != TEST_RESULT_SUCCESS) {
+		return res;
+	}
+
+	/*
+	 * Fill all 3 world's FPU/SIMD state regs with some known values in the
+	 * beginning to have something later to compare to.
+	 */
+	fpu_state_fill_regs_and_template(&fpu_temp_ns);
+	if (!fpu_fill_rl()) {
+		ERROR("fpu_fill_rl error\n");
+		goto destroy_realm;
+	}
+
+	for (uint32_t i = 0; i < 1000; i++) {
+		cmd = rand() % cmd_max;
+		if ((cmd == old_cmd) || cmd == CMD_MAX_THREE_WORLD) {
+			continue;
+		}
+		old_cmd = cmd;
+
+		switch (cmd) {
+		case CMD_SIMD_NS_FILL:
+			/* Non secure world fill FPU/SIMD state registers */
+			fpu_state_fill_regs_and_template(&fpu_temp_ns);
+			break;
+		case CMD_SIMD_NS_CMP:
+			/* Normal world verify its FPU/SIMD state registers data */
+			if (!fpu_state_compare_template(&fpu_temp_ns)) {
+				ERROR("%s failed %d\n", __func__, __LINE__);
+				goto destroy_realm;
+			}
+			break;
+		case CMD_SIMD_SEC_FILL:
+			/* secure world fill FPU/SIMD state registers */
+			if (!fpu_fill_sec()) {
+				goto destroy_realm;
+			}
+			break;
+		case CMD_SIMD_SEC_CMP:
+			/* Secure world verify its FPU/SIMD state registers data */
+			if (!fpu_cmp_sec()) {
+				goto destroy_realm;
+			}
+			break;
+		case CMD_SIMD_RL_FILL:
+			/* Realm R-EL1 world fill FPU/SIMD state registers */
+			if (!fpu_fill_rl()) {
+				goto destroy_realm;
+			}
+			break;
+		case CMD_SIMD_RL_CMP:
+			/* Realm R-EL1 world verify its FPU/SIMD state registers data */
+			if (!fpu_cmp_rl()) {
+				goto destroy_realm;
+			}
+			break;
+		default:
+			break;
+		}
+	}
+
+	if (!host_destroy_realm()) {
+		ERROR("host_destroy_realm error\n");
+		return TEST_RESULT_FAIL;
+	}
+	return TEST_RESULT_SUCCESS;
+destroy_realm:
+	host_destroy_realm();
+	return TEST_RESULT_FAIL;
+}
diff --git a/tftf/tests/tests-realm-payload.mk b/tftf/tests/tests-realm-payload.mk
index b6f45ed..b3b7d4a 100644
--- a/tftf/tests/tests-realm-payload.mk
+++ b/tftf/tests/tests-realm-payload.mk
@@ -4,6 +4,7 @@
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
+ifeq (${ARCH},aarch64)
 TFTF_INCLUDES +=							\
 	-Iinclude/runtime_services/host_realm_managment
 
@@ -33,3 +34,9 @@
 	$(addprefix lib/heap/,						\
 		page_alloc.c						\
 	)
+
+TESTS_SOURCES	+=							\
+	$(addprefix lib/extensions/fpu/,				\
+		fpu.c							\
+	)
+endif
diff --git a/tftf/tests/tests-realm-payload.xml b/tftf/tests/tests-realm-payload.xml
index b3d86ad..7518934 100644
--- a/tftf/tests/tests-realm-payload.xml
+++ b/tftf/tests/tests-realm-payload.xml
@@ -30,5 +30,7 @@
 	  function="host_realm_pmuv3_overflow_interrupt" />
 	  <testcase name="Test Secure interrupt can preempt Realm EL1"
 	  function="host_realm_sec_interrupt_can_preempt_rl" />
+	  <testcase name="Check that FPU state registers context is preserved in RL/SE/NS"
+	  function="host_realm_fpu_access_in_rl_ns_se" />
   </testsuite>
 </testsuites>