test(realm): unify SIMD test cases

There are two testcases host_realm_fpu_access_in_rl_ns_se and
host_and_realm_check_simd which verifies SIMD state across different
world switch. This change unifies both these testcases to one test case
host_realm_swd_check_simd so that new enhancements or fixes can be done
in one place.

This change also increases the code coverage by removing the restriction
on the need for both secure partition and realm payload as a
prerequisite.

Signed-off-by: Arunachalam Ganapathy <arunachalam.ganapathy@arm.com>
Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
Change-Id: I210acdff9e584cb392a2f1919dce04971fd2fc7a
diff --git a/tftf/tests/runtime_services/realm_payload/host_realm_payload_simd_tests.c b/tftf/tests/runtime_services/realm_payload/host_realm_payload_simd_tests.c
index 44b872f..5304180 100644
--- a/tftf/tests/runtime_services/realm_payload/host_realm_payload_simd_tests.c
+++ b/tftf/tests/runtime_services/realm_payload/host_realm_payload_simd_tests.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2023, Arm Limited. All rights reserved.
+ * Copyright (c) 2023-2024, Arm Limited. All rights reserved.
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
@@ -18,53 +18,17 @@
 #include <host_realm_simd.h>
 #include <host_shared_data.h>
 
+#include "host_realm_simd_common.h"
+
 #define NS_SVE_OP_ARRAYSIZE		1024U
 #define SVE_TEST_ITERATIONS		50U
 
-/* Min test iteration count for 'host_and_realm_check_simd' test */
-#define TEST_ITERATIONS_MIN	(16U)
-
-/* Number of FPU configs: none */
-#define NUM_FPU_CONFIGS		(0U)
-
-/* Number of SVE configs: SVE_VL, SVE hint */
-#define NUM_SVE_CONFIGS		(2U)
-
-/* Number of SME configs: SVE_SVL, FEAT_FA64, Streaming mode */
-#define NUM_SME_CONFIGS		(3U)
-
-#define NS_NORMAL_SVE			0x1U
-#define NS_STREAMING_SVE		0x2U
-
-typedef enum security_state {
-	NONSECURE_WORLD = 0U,
-	REALM_WORLD,
-	SECURITY_STATE_MAX
-} security_state_t;
-
-typedef enum {
-	TEST_FPU = 0U,
-	TEST_SVE,
-	TEST_SME,
-} simd_test_t;
-
 static int ns_sve_op_1[NS_SVE_OP_ARRAYSIZE];
 static int ns_sve_op_2[NS_SVE_OP_ARRAYSIZE];
 
-static sve_z_regs_t ns_sve_z_regs_write;
-static sve_z_regs_t ns_sve_z_regs_read;
-
-static sve_p_regs_t ns_sve_p_regs_write;
-static sve_p_regs_t ns_sve_p_regs_read;
-
-static sve_ffr_regs_t ns_sve_ffr_regs_write;
-static sve_ffr_regs_t ns_sve_ffr_regs_read;
-
-static fpu_q_reg_t ns_fpu_q_regs_write[FPU_Q_COUNT];
-static fpu_q_reg_t ns_fpu_q_regs_read[FPU_Q_COUNT];
-
-static fpu_cs_regs_t ns_fpu_cs_regs_write;
-static fpu_cs_regs_t ns_fpu_cs_regs_read;
+/* Defined in host_realm_simd_common.c */
+extern sve_z_regs_t ns_sve_z_regs_write;
+extern sve_z_regs_t ns_sve_z_regs_read;
 
 static struct realm realm;
 
@@ -86,40 +50,6 @@
 		}								\
 	} while (false)
 
-static test_result_t host_create_sve_realm_payload(bool sve_en, uint8_t sve_vq)
-{
-	u_register_t feature_flag = 0UL;
-	long sl = RTT_MIN_LEVEL;
-	u_register_t rec_flag[1] = {RMI_RUNNABLE};
-
-	if (is_feat_52b_on_4k_2_supported() == true) {
-		feature_flag = RMI_FEATURE_REGISTER_0_LPA2;
-		sl = RTT_MIN_LEVEL_LPA2;
-	}
-
-	if (sve_en) {
-		feature_flag |= RMI_FEATURE_REGISTER_0_SVE_EN |
-				INPLACE(FEATURE_SVE_VL, sve_vq);
-	}
-
-	/* Initialise Realm payload */
-	if (!host_create_activate_realm_payload(&realm,
-				       (u_register_t)REALM_IMAGE_BASE,
-				       (u_register_t)PAGE_POOL_BASE,
-				       (u_register_t)PAGE_POOL_MAX_SIZE,
-				       feature_flag, sl, rec_flag, 1U)) {
-		return TEST_RESULT_FAIL;
-	}
-
-	/* Create shared memory between Host and Realm */
-	if (!host_create_shared_mem(&realm, NS_REALM_SHARED_MEM_BASE,
-				    NS_REALM_SHARED_MEM_SIZE)) {
-		return TEST_RESULT_FAIL;
-	}
-
-	return TEST_RESULT_SUCCESS;
-}
-
 /*
  * RMI should report SVE VL in RMI features and it must be the same value as the
  * max SVE VL seen by the NS world.
@@ -169,7 +99,7 @@
 
 	sve_vq = EXTRACT(RMI_FEATURE_REGISTER_0_SVE_VL, rmi_feat_reg0);
 
-	rc = host_create_sve_realm_payload(true, sve_vq);
+	rc = host_create_sve_realm_payload(&realm, true, sve_vq);
 	if (rc != TEST_RESULT_SUCCESS) {
 		ERROR("Failed to create Realm with SVE\n");
 		return TEST_RESULT_FAIL;
@@ -220,7 +150,7 @@
 	 * Pass a sve_vq that is greater than the value supported by RMM
 	 * and check whether creating Realm fails
 	 */
-	rc = host_create_sve_realm_payload(true, (sve_vq + 1));
+	rc = host_create_sve_realm_payload(&realm, true, (sve_vq + 1));
 	if (rc == TEST_RESULT_SUCCESS) {
 		ERROR("Error: Realm created with invalid SVE VL %u\n", (sve_vq + 1));
 		host_destroy_realm(&realm);
@@ -246,7 +176,7 @@
 		sve_vq = EXTRACT(RMI_FEATURE_REGISTER_0_SVE_VL, rmi_feat_reg0);
 	}
 
-	rc = host_create_sve_realm_payload(sve_en, sve_vq);
+	rc = host_create_sve_realm_payload(&realm, sve_en, sve_vq);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -327,7 +257,7 @@
 
 	sve_vq = EXTRACT(RMI_FEATURE_REGISTER_0_SVE_VL, rmi_feat_reg0);
 
-	rc = host_create_sve_realm_payload(true, sve_vq);
+	rc = host_create_sve_realm_payload(&realm, true, sve_vq);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -382,7 +312,7 @@
 
 	vq = EXTRACT(RMI_FEATURE_REGISTER_0_SVE_VL, rmi_feat_reg0);
 
-	rc = host_create_sve_realm_payload(true, vq);
+	rc = host_create_sve_realm_payload(&realm, true, vq);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -459,7 +389,7 @@
 	unsigned int i;
 	int val;
 
-	rc = host_create_sve_realm_payload(realm_sve_en, realm_sve_vq);
+	rc = host_create_sve_realm_payload(&realm, realm_sve_en, realm_sve_vq);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -621,7 +551,7 @@
 	sve_config_vq(SVE_VQ_ARCH_MIN);
 
 	/* 3. Create Realm with max VQ (higher than NS SVE VQ) */
-	rc = host_create_sve_realm_payload(true, sve_vq);
+	rc = host_create_sve_realm_payload(&realm, true, sve_vq);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -672,7 +602,7 @@
 	SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP();
 	SKIP_TEST_IF_SVE_NOT_SUPPORTED();
 
-	rc = host_create_sve_realm_payload(false, 0);
+	rc = host_create_sve_realm_payload(&realm, false, 0);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -693,496 +623,6 @@
 	return rc;
 }
 
-/* Generate random values and write it to SVE Z, P and FFR registers */
-static void ns_sve_write_rand(void)
-{
-	bool has_ffr = true;
-
-	if (is_feat_sme_supported() && sme_smstat_sm() &&
-	    !sme_feat_fa64_enabled()) {
-		has_ffr = false;
-	}
-
-	sve_z_regs_write_rand(&ns_sve_z_regs_write);
-	sve_p_regs_write_rand(&ns_sve_p_regs_write);
-	if (has_ffr) {
-		sve_ffr_regs_write_rand(&ns_sve_ffr_regs_write);
-	}
-}
-
-/* Read SVE Z, P and FFR registers and compare it with the last written values */
-static test_result_t ns_sve_read_and_compare(void)
-{
-	test_result_t rc = TEST_RESULT_SUCCESS;
-	uint64_t bitmap;
-	bool has_ffr = true;
-
-	if (is_feat_sme_supported() && sme_smstat_sm() &&
-	    !sme_feat_fa64_enabled()) {
-		has_ffr = false;
-	}
-
-	/* Clear old state */
-	memset((void *)&ns_sve_z_regs_read, 0, sizeof(ns_sve_z_regs_read));
-	memset((void *)&ns_sve_p_regs_read, 0, sizeof(ns_sve_p_regs_read));
-	memset((void *)&ns_sve_ffr_regs_read, 0, sizeof(ns_sve_ffr_regs_read));
-
-	/* Read Z, P, FFR registers to compare it with the last written values */
-	sve_z_regs_read(&ns_sve_z_regs_read);
-	sve_p_regs_read(&ns_sve_p_regs_read);
-	if (has_ffr) {
-		sve_ffr_regs_read(&ns_sve_ffr_regs_read);
-	}
-
-	bitmap = sve_z_regs_compare(&ns_sve_z_regs_write, &ns_sve_z_regs_read);
-	if (bitmap != 0UL) {
-		ERROR("SVE Z regs compare failed (bitmap: 0x%016llx)\n",
-		      bitmap);
-		rc = TEST_RESULT_FAIL;
-	}
-
-	bitmap = sve_p_regs_compare(&ns_sve_p_regs_write, &ns_sve_p_regs_read);
-	if (bitmap != 0UL) {
-		ERROR("SVE P regs compare failed (bitmap: 0x%016llx)\n",
-		      bitmap);
-		rc = TEST_RESULT_FAIL;
-	}
-
-	if (has_ffr) {
-		bitmap = sve_ffr_regs_compare(&ns_sve_ffr_regs_write,
-					      &ns_sve_ffr_regs_read);
-		if (bitmap != 0) {
-			ERROR("SVE FFR regs compare failed "
-			      "(bitmap: 0x%016llx)\n", bitmap);
-			rc = TEST_RESULT_FAIL;
-		}
-	}
-
-	return rc;
-}
-
-/*
- * Generate random values and write it to Streaming SVE Z, P and FFR registers.
- */
-static void ns_sme_write_rand(void)
-{
-	/*
-	 * TODO: more SME specific registers like ZA, ZT0 can be included later.
-	 */
-
-	/* Fill SVE registers in normal or streaming SVE mode */
-	ns_sve_write_rand();
-}
-
-/*
- * Read streaming SVE Z, P and FFR registers and compare it with the last
- * written values
- */
-static test_result_t ns_sme_read_and_compare(void)
-{
-	/*
-	 * TODO: more SME specific registers like ZA, ZT0 can be included later.
-	 */
-
-	/* Compares SVE registers in normal or streaming SVE mode */
-	return ns_sve_read_and_compare();
-}
-
-static char *simd_type_to_str(simd_test_t type)
-{
-	if (type == TEST_FPU) {
-		return "FPU";
-	} else if (type == TEST_SVE) {
-		return "SVE";
-	} else if (type == TEST_SME) {
-		return "SME";
-	} else {
-		return "UNKNOWN";
-	}
-}
-
-static void ns_simd_print_cmd_config(bool cmd, simd_test_t type)
-{
-	char __unused *tstr = simd_type_to_str(type);
-	char __unused *cstr = cmd ? "write rand" : "read and compare";
-
-	if (type == TEST_SME) {
-		if (sme_smstat_sm()) {
-			INFO("TFTF: NS [%s] %s. Config: smcr: 0x%llx, SM: on\n",
-			     tstr, cstr, (uint64_t)read_smcr_el2());
-		} else {
-			INFO("TFTF: NS [%s] %s. Config: smcr: 0x%llx, "
-			     "zcr: 0x%llx sve_hint: %d SM: off\n", tstr, cstr,
-			     (uint64_t)read_smcr_el2(),
-			     (uint64_t)sve_read_zcr_elx(),
-			     tftf_smc_get_sve_hint());
-		}
-	} else if (type == TEST_SVE) {
-		INFO("TFTF: NS [%s] %s. Config: zcr: 0x%llx, sve_hint: %d\n",
-		     tstr, cstr, (uint64_t)sve_read_zcr_elx(),
-		     tftf_smc_get_sve_hint());
-	} else {
-		INFO("TFTF: NS [%s] %s\n", tstr, cstr);
-	}
-}
-
-/*
- * Randomly select TEST_SME or TEST_FPU. For TEST_SME, randomly select below
- * configurations:
- * - enable/disable streaming mode
- *   For streaming mode:
- *   - enable or disable FA64
- *   - select random streaming vector length
- *   For normal SVE mode:
- *   - select random normal SVE vector length
- */
-static simd_test_t ns_sme_select_random_config(void)
-{
-	simd_test_t type;
-	static unsigned int counter;
-
-	/* Use a static counter to mostly select TEST_SME case. */
-	if ((counter % 8U) != 0) {
-		/* Use counter to toggle between Streaming mode on or off */
-		if (is_armv8_2_sve_present() && ((counter % 2U) != 0)) {
-			sme_smstop(SMSTOP_SM);
-			sve_config_vq(SVE_GET_RANDOM_VQ);
-
-			if ((counter % 3U) != 0) {
-				tftf_smc_set_sve_hint(true);
-			} else {
-				tftf_smc_set_sve_hint(false);
-			}
-		} else {
-			sme_smstart(SMSTART_SM);
-			sme_config_svq(SME_GET_RANDOM_SVQ);
-
-			if ((counter % 3U) != 0) {
-				sme_enable_fa64();
-			} else {
-				sme_disable_fa64();
-			}
-		}
-		type = TEST_SME;
-	} else {
-		type = TEST_FPU;
-	}
-	counter++;
-
-	return type;
-}
-
-/*
- * Randomly select TEST_SVE or TEST_FPU. For TEST_SVE, configure zcr_el2 with
- * random vector length and randomly enable or disable SMC SVE hint bit.
- */
-static simd_test_t ns_sve_select_random_config(void)
-{
-	simd_test_t type;
-	static unsigned int counter;
-
-	/* Use a static counter to mostly select TEST_SVE case. */
-	if ((counter % 4U) != 0) {
-		sve_config_vq(SVE_GET_RANDOM_VQ);
-
-		if ((counter % 2U) != 0) {
-			tftf_smc_set_sve_hint(true);
-		} else {
-			tftf_smc_set_sve_hint(false);
-		}
-
-		type = TEST_SVE;
-	} else {
-		type = TEST_FPU;
-	}
-	counter++;
-
-	return type;
-}
-
-/*
- * Configure NS world SIMD. Randomly choose to test SVE or FPU registers if
- * system supports SVE.
- *
- * Returns either TEST_FPU or TEST_SVE or TEST_SME
- */
-static simd_test_t ns_simd_select_random_config(void)
-{
-	simd_test_t type;
-
-	/* cleanup old config for SME */
-	if (is_feat_sme_supported()) {
-		sme_smstop(SMSTOP_SM);
-		sme_enable_fa64();
-	}
-
-	/* Cleanup old config for SVE */
-	if (is_armv8_2_sve_present()) {
-		tftf_smc_set_sve_hint(false);
-	}
-
-	if (is_armv8_2_sve_present() && is_feat_sme_supported()) {
-		if (rand() % 2) {
-			type = ns_sme_select_random_config();
-		} else {
-			type = ns_sve_select_random_config();
-		}
-	} else if (is_feat_sme_supported()) {
-		type = ns_sme_select_random_config();
-	} else if (is_armv8_2_sve_present()) {
-		type = ns_sve_select_random_config();
-	} else {
-		type = TEST_FPU;
-	}
-
-	return type;
-}
-
-/* Select random NS SIMD config and write random values to its registers */
-static simd_test_t ns_simd_write_rand(void)
-{
-	simd_test_t type;
-
-	type = ns_simd_select_random_config();
-
-	ns_simd_print_cmd_config(true, type);
-
-	if (type == TEST_SME) {
-		ns_sme_write_rand();
-	} else if (type == TEST_SVE) {
-		ns_sve_write_rand();
-	} else {
-		fpu_q_regs_write_rand(ns_fpu_q_regs_write);
-	}
-
-	/* fpcr, fpsr common to all configs */
-	fpu_cs_regs_write_rand(&ns_fpu_cs_regs_write);
-
-	return type;
-}
-
-/* Read and compare the NS SIMD registers with the last written values */
-static test_result_t ns_simd_read_and_compare(simd_test_t type)
-{
-	test_result_t rc = TEST_RESULT_SUCCESS;
-
-	ns_simd_print_cmd_config(false, type);
-
-	if (type == TEST_SME) {
-		rc = ns_sme_read_and_compare();
-	} else if (type == TEST_SVE) {
-		rc = ns_sve_read_and_compare();
-	} else {
-		fpu_q_regs_read(ns_fpu_q_regs_read);
-		if (fpu_q_regs_compare(ns_fpu_q_regs_write,
-				       ns_fpu_q_regs_read)) {
-			ERROR("FPU Q registers compare failed\n");
-			rc = TEST_RESULT_FAIL;
-		}
-	}
-
-	/* fpcr, fpsr common to all configs */
-	fpu_cs_regs_read(&ns_fpu_cs_regs_read);
-	if (fpu_cs_regs_compare(&ns_fpu_cs_regs_write, &ns_fpu_cs_regs_read)) {
-		ERROR("FPCR/FPSR registers compare failed\n");
-		rc = TEST_RESULT_FAIL;
-	}
-
-	return rc;
-}
-
-/* Select random Realm SIMD config and write random values to its registers */
-static simd_test_t rl_simd_write_rand(bool rl_sve_en)
-{
-	enum realm_cmd rl_fill_cmd;
-	simd_test_t type;
-	bool __unused rc;
-
-	/* Select random commands to test. SVE or FPU registers in Realm */
-	if (rl_sve_en && (rand() % 2)) {
-		type = TEST_SVE;
-	} else {
-		type = TEST_FPU;
-	}
-
-	INFO("TFTF: RL [%s] write random\n", simd_type_to_str(type));
-	if (type == TEST_SVE) {
-		rl_fill_cmd = REALM_SVE_FILL_REGS;
-	} else {
-		rl_fill_cmd = REALM_REQ_FPU_FILL_CMD;
-	}
-
-	rc = host_enter_realm_execute(&realm, rl_fill_cmd, RMI_EXIT_HOST_CALL, 0U);
-	assert(rc);
-
-	return type;
-}
-
-/* Read and compare the Realm SIMD registers with the last written values */
-static bool rl_simd_read_and_compare(simd_test_t type)
-{
-	enum realm_cmd rl_cmp_cmd;
-
-	INFO("TFTF: RL [%s] read and compare\n", simd_type_to_str(type));
-	if (type == TEST_SVE) {
-		rl_cmp_cmd = REALM_SVE_CMP_REGS;
-	} else {
-		rl_cmp_cmd = REALM_REQ_FPU_CMP_CMD;
-	}
-
-	return host_enter_realm_execute(&realm, rl_cmp_cmd, RMI_EXIT_HOST_CALL,
-					0U);
-}
-
-/*
- * This test case verifies whether various SIMD related registers like Q[0-31],
- * FPCR, FPSR, Z[0-31], P[0-15], FFR are preserved by RMM during world switch
- * between NS world and Realm world.
- *
- * Randomly verify FPU registers or SVE registers if the system supports SVE.
- * Within SVE, randomly configure SVE vector length.
- *
- * This testcase runs on below configs:
- * - SVE only
- * - SME only
- * - with SVE and SME
- * - without SVE and SME
- */
-test_result_t host_and_realm_check_simd(void)
-{
-	u_register_t rmi_feat_reg0;
-	test_result_t rc;
-	uint8_t sve_vq;
-	bool sve_en;
-	security_state_t sec_state;
-	simd_test_t ns_simd_type, rl_simd_type;
-	unsigned int test_iterations;
-	unsigned int num_simd_types;
-	unsigned int num_simd_configs;
-
-	SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP();
-
-	if (host_rmi_features(0UL, &rmi_feat_reg0) != REALM_SUCCESS) {
-		ERROR("Failed to get RMI feat_reg0\n");
-		return TEST_RESULT_FAIL;
-	}
-
-	sve_en = rmi_feat_reg0 & RMI_FEATURE_REGISTER_0_SVE_EN;
-	sve_vq = EXTRACT(RMI_FEATURE_REGISTER_0_SVE_VL, rmi_feat_reg0);
-
-	/* Create Realm with SVE enabled if RMI features supports it */
-	INFO("TFTF: create realm sve_en/sve_vq: %d/%d\n", sve_en, sve_vq);
-	rc = host_create_sve_realm_payload(sve_en, sve_vq);
-	if (rc != TEST_RESULT_SUCCESS) {
-		return rc;
-	}
-
-	/*
-	 * Randomly select and configure NS simd context to test. And fill it
-	 * with random values.
-	 */
-	ns_simd_type = ns_simd_write_rand();
-
-	/*
-	 * Randomly select and configure Realm simd context to test. Enter realm
-	 * and fill simd context with random values.
-	 */
-	rl_simd_type = rl_simd_write_rand(sve_en);
-	sec_state = REALM_WORLD;
-
-	/*
-	 * Find out test iterations based on if SVE is enabled and the number of
-	 * configurations available in the SVE.
-	 */
-
-	/* FPU is always available */
-	num_simd_types = 1U;
-	num_simd_configs = NUM_FPU_CONFIGS;
-
-	if (is_armv8_2_sve_present()) {
-		num_simd_types += 1;
-		num_simd_configs += NUM_SVE_CONFIGS;
-	}
-
-	if (is_feat_sme_supported()) {
-		num_simd_types += 1;
-		num_simd_configs += NUM_SME_CONFIGS;
-	}
-
-	if (num_simd_configs) {
-		test_iterations = TEST_ITERATIONS_MIN * num_simd_types *
-			num_simd_configs;
-	} else {
-		test_iterations = TEST_ITERATIONS_MIN * num_simd_types;
-	}
-
-	for (uint32_t i = 0U; i < test_iterations; i++) {
-		if (sec_state == NONSECURE_WORLD) {
-			sec_state = REALM_WORLD;
-		} else {
-			sec_state = NONSECURE_WORLD;
-		}
-
-		switch (sec_state) {
-		case NONSECURE_WORLD:
-			/*
-			 * Read NS simd context and compare it with last written
-			 * context.
-			 */
-			rc = ns_simd_read_and_compare(ns_simd_type);
-			if (rc != TEST_RESULT_SUCCESS) {
-				goto rm_realm;
-			}
-
-			/*
-			 * Randomly select and configure NS simd context. And
-			 * fill it with random values for the next compare.
-			 */
-			ns_simd_type = ns_simd_write_rand();
-			break;
-		case REALM_WORLD:
-			/*
-			 * Enter Realm and read the simd context and compare it
-			 * with last written context.
-			 */
-			if (!rl_simd_read_and_compare(rl_simd_type)) {
-				ERROR("%s failed %d\n", __func__, __LINE__);
-				rc = TEST_RESULT_FAIL;
-				goto rm_realm;
-			}
-
-			/*
-			 * Randomly select and configure Realm simd context to
-			 * test. Enter realm and fill simd context with random
-			 * values for the next compare.
-			 */
-			rl_simd_type = rl_simd_write_rand(sve_en);
-			break;
-		default:
-			break;
-		}
-	}
-
-	rc = TEST_RESULT_SUCCESS;
-rm_realm:
-	/* Cleanup old config */
-	if (is_feat_sme_supported()) {
-		sme_smstop(SMSTOP_SM);
-		sme_enable_fa64();
-	}
-
-	/* Cleanup old config */
-	if (is_armv8_2_sve_present()) {
-		tftf_smc_set_sve_hint(false);
-	}
-
-	if (!host_destroy_realm(&realm)) {
-		return TEST_RESULT_FAIL;
-	}
-
-	return rc;
-}
-
 /*
  * Create a Realm and check SME specific ID registers. Realm must report SME
  * not present in ID_AA64PFR1_EL1 and no SME features present in
@@ -1197,7 +637,7 @@
 
 	SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP();
 
-	rc = host_create_sve_realm_payload(false, 0);
+	rc = host_create_sve_realm_payload(&realm, false, 0);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -1240,7 +680,7 @@
 	SKIP_TEST_IF_SME_NOT_SUPPORTED();
 	SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP();
 
-	rc = host_create_sve_realm_payload(false, 0);
+	rc = host_create_sve_realm_payload(&realm, false, 0);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
@@ -1289,7 +729,7 @@
 		sve_vq = 0;
 	}
 
-	rc = host_create_sve_realm_payload(sve_en, sve_vq);
+	rc = host_create_sve_realm_payload(&realm, sve_en, sve_vq);
 	if (rc != TEST_RESULT_SUCCESS) {
 		return rc;
 	}
diff --git a/tftf/tests/runtime_services/realm_payload/host_realm_simd_common.c b/tftf/tests/runtime_services/realm_payload/host_realm_simd_common.c
new file mode 100644
index 0000000..9f666a9
--- /dev/null
+++ b/tftf/tests/runtime_services/realm_payload/host_realm_simd_common.c
@@ -0,0 +1,55 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_features.h>
+#include <tftf.h>
+#include <host_realm_rmi.h>
+#include <host_realm_helper.h>
+#include <host_realm_mem_layout.h>
+#include <lib/extensions/sve.h>
+
+#include "host_realm_simd_common.h"
+
+/*
+ * Testcases in host_realm_spm.c, host_realm_payload_simd_tests.c uses these
+ * common variables.
+ */
+sve_z_regs_t ns_sve_z_regs_write;
+sve_z_regs_t ns_sve_z_regs_read;
+
+test_result_t host_create_sve_realm_payload(struct realm *realm, bool sve_en,
+					    uint8_t sve_vq)
+{
+	u_register_t feature_flag = 0UL;
+	long sl = RTT_MIN_LEVEL;
+	u_register_t rec_flag[1] = {RMI_RUNNABLE};
+
+	if (is_feat_52b_on_4k_2_supported() == true) {
+		feature_flag = RMI_FEATURE_REGISTER_0_LPA2;
+		sl = RTT_MIN_LEVEL_LPA2;
+	}
+
+	if (sve_en) {
+		feature_flag |= RMI_FEATURE_REGISTER_0_SVE_EN |
+				INPLACE(FEATURE_SVE_VL, sve_vq);
+	}
+
+	/* Initialise Realm payload */
+	if (!host_create_activate_realm_payload(realm,
+				       (u_register_t)REALM_IMAGE_BASE,
+				       (u_register_t)PAGE_POOL_BASE,
+				       (u_register_t)PAGE_POOL_MAX_SIZE,
+				       feature_flag, sl, rec_flag, 1U)) {
+		return TEST_RESULT_FAIL;
+	}
+
+	/* Create shared memory between Host and Realm */
+	if (!host_create_shared_mem(realm, NS_REALM_SHARED_MEM_BASE,
+				    NS_REALM_SHARED_MEM_SIZE)) {
+		return TEST_RESULT_FAIL;
+	}
+
+	return TEST_RESULT_SUCCESS;
+}
diff --git a/tftf/tests/runtime_services/realm_payload/host_realm_simd_common.h b/tftf/tests/runtime_services/realm_payload/host_realm_simd_common.h
new file mode 100644
index 0000000..377c85b
--- /dev/null
+++ b/tftf/tests/runtime_services/realm_payload/host_realm_simd_common.h
@@ -0,0 +1,15 @@
+/*
+ * Copyright (c) 2024, Arm Limited. All rights reserved.
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef HOST_REALM_COMMON_H
+#define HOST_REALM_COMMON_h
+
+#define NS_NORMAL_SVE		0x1U
+#define NS_STREAMING_SVE	0x2U
+
+test_result_t host_create_sve_realm_payload(struct realm *realm, bool sve_en,
+					    uint8_t sve_vq);
+
+#endif
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 589888e..8649014 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-2023, Arm Limited. All rights reserved.
+ * Copyright (c) 2022-2024, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -15,6 +15,9 @@
 #include <host_shared_data.h>
 #include <spm_test_helpers.h>
 #include <test_helpers.h>
+#include <tftf_lib.h>
+
+#include "host_realm_simd_common.h"
 
 #define REALM_TIME_SLEEP	300U
 #define SENDER HYP_ID
@@ -23,9 +26,26 @@
 static struct mailbox_buffers mb;
 static bool secure_mailbox_initialised;
 
-static fpu_state_t ns_fpu_state_write;
-static fpu_state_t ns_fpu_state_read;
-static struct realm realm;
+/*
+ * Min test iteration count for 'host_and_realm_check_simd' and
+ * 'host_realm_swd_check_simd' tests.
+ */
+#define TEST_ITERATIONS_MIN	(16U)
+
+/* Number of FPU configs: none */
+#define NUM_FPU_CONFIGS		(0U)
+
+/* Number of SVE configs: SVE_VL, SVE hint */
+#define NUM_SVE_CONFIGS		(2U)
+
+/* Number of SME configs: SVE_SVL, FEAT_FA64, Streaming mode */
+#define NUM_SME_CONFIGS		(3U)
+
+typedef enum {
+	TEST_FPU = 0U,
+	TEST_SVE,
+	TEST_SME,
+} simd_test_t;
 
 typedef enum security_state {
 	NONSECURE_WORLD = 0U,
@@ -34,48 +54,65 @@
 	SECURITY_STATE_MAX
 } security_state_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_sp(void)
+/* Defined in host_realm_simd_common.c */
+extern sve_z_regs_t ns_sve_z_regs_write;
+extern sve_z_regs_t ns_sve_z_regs_read;
+
+static sve_p_regs_t ns_sve_p_regs_write;
+static sve_p_regs_t ns_sve_p_regs_read;
+
+static sve_ffr_regs_t ns_sve_ffr_regs_write;
+static sve_ffr_regs_t ns_sve_ffr_regs_read;
+
+static fpu_q_reg_t ns_fpu_q_regs_write[FPU_Q_COUNT];
+static fpu_q_reg_t ns_fpu_q_regs_read[FPU_Q_COUNT];
+
+static fpu_cs_regs_t ns_fpu_cs_regs_write;
+static fpu_cs_regs_t ns_fpu_cs_regs_read;
+
+static struct realm realm;
+
+/* This function helps to initialise secure_mailbox if FFA is supported  */
+static test_result_t init_secure_partition(void)
 {
+	/* Verify that FF-A is there and that it has the correct version. */
+	SKIP_TEST_IF_FFA_VERSION_LESS_THAN(1, 1);
+
 	if (!secure_mailbox_initialised) {
 		GET_TFTF_MAILBOX(mb);
 		CHECK_SPMC_TESTING_SETUP(1, 1, expected_sp_uuids);
 		secure_mailbox_initialised = true;
 	}
+
 	return TEST_RESULT_SUCCESS;
 }
 
-static test_result_t init_realm(void)
+static test_result_t init_realm_payload(struct realm *realm, bool *sve_en_ret)
 {
-	u_register_t rec_flag[1] = {RMI_RUNNABLE};
-	u_register_t feature_flag = 0U;
-	long sl = RTT_MIN_LEVEL;
+	u_register_t rmi_feat_reg0;
+	test_result_t rc;
+	uint8_t sve_vq;
+	bool sve_en;
 
-	if (is_feat_52b_on_4k_2_supported() == true) {
-		feature_flag = RMI_FEATURE_REGISTER_0_LPA2;
-		sl = RTT_MIN_LEVEL_LPA2;
-	}
+	SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP();
 
-	/*
-	 * Initialise Realm payload
-	 */
-	if (!host_create_activate_realm_payload(&realm, (u_register_t)REALM_IMAGE_BASE,
-			(u_register_t)PAGE_POOL_BASE,
-			(u_register_t)PAGE_POOL_MAX_SIZE,
-			feature_flag, sl, rec_flag, 1U)) {
+	if (host_rmi_features(0UL, &rmi_feat_reg0) != REALM_SUCCESS) {
+		ERROR("Failed to get RMI feat_reg0\n");
 		return TEST_RESULT_FAIL;
 	}
 
-	/*
-	 * Create shared memory between Host and Realm
-	 */
-	if (!host_create_shared_mem(&realm, NS_REALM_SHARED_MEM_BASE,
-			NS_REALM_SHARED_MEM_SIZE)) {
-		return TEST_RESULT_FAIL;
+	sve_en = rmi_feat_reg0 & RMI_FEATURE_REGISTER_0_SVE_EN;
+	sve_vq = EXTRACT(RMI_FEATURE_REGISTER_0_SVE_VL, rmi_feat_reg0);
+
+	/* Create Realm with SVE enabled if RMI features supports it */
+	INFO("TFTF: create realm sve_en/sve_vq: %d/%d\n", sve_en, sve_vq);
+	rc = host_create_sve_realm_payload(realm, sve_en, sve_vq);
+	if (rc != TEST_RESULT_SUCCESS) {
+		return rc;
+	}
+
+	if (sve_en_ret != NULL) {
+		*sve_en_ret = sve_en;
 	}
 
 	return TEST_RESULT_SUCCESS;
@@ -91,59 +128,6 @@
 	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, REALM_REQ_FPU_FILL_CMD, RMI_EXIT_HOST_CALL, 0U)) {
-		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, REALM_REQ_FPU_CMP_CMD, RMI_EXIT_HOST_CALL, 0U)) {
-		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.
@@ -190,18 +174,12 @@
 	struct ffa_value ret_values;
 	test_result_t res;
 
-	/* Verify RME is present and RMM is not TRP */
-	SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP();
-
-	/* Verify that FFA is there and that it has the correct version. */
-	SKIP_TEST_IF_FFA_VERSION_LESS_THAN(1, 1);
-
-	res = init_sp();
+	res = init_secure_partition();
 	if (res != TEST_RESULT_SUCCESS) {
 		return res;
 	}
 
-	res = init_realm();
+	res = init_realm_payload(&realm, NULL);
 	if (res != TEST_RESULT_SUCCESS) {
 		return res;
 	}
@@ -270,17 +248,21 @@
 
 /* Choose a random security state that is different from the 'current' state */
 static security_state_t get_random_security_state(security_state_t current,
-						  bool is_sp_present)
+						  bool is_sp_present,
+						  bool is_rl_present)
 {
 	security_state_t next;
 
 	/*
-	 * 3 world config: Switch between NS world and Realm world as Secure
-	 * world is not enabled or SP is not loaded.
+	 * 3 world config:
+	 * Switch between NS world and Realm world if Secure world is not
+	 * enabled or SP is not loaded. or
+	 * Switch between NS world and Secure world if Realm world is not
+	 * enabled.
 	 */
-	if (!is_sp_present) {
+	if (is_sp_present ^ is_rl_present) {
 		if (current == NONSECURE_WORLD) {
-			return REALM_WORLD;
+			return is_rl_present ? REALM_WORLD : SECURE_WORLD;
 		} else {
 			return NONSECURE_WORLD;
 		}
@@ -302,120 +284,556 @@
 	return next;
 }
 
-/*
- * Test whether FPU/SIMD state (32 SIMD vectors, FPCR and FPSR registers) are
- * preserved during a random context switch between Secure/Non-Secure/Realm world
- *
- * Below steps are performed by this test:
- *
- * Init:
- * Fill FPU registers with random values in
- * 1. NS World (NS-EL2)
- * 2. Realm world (R-EL1)
- * 3. Secure world (S-EL1) (if SP loaded)
- *
- * Test loop:
- *  security_state_next = get_random_security_state(current, is_sp_present)
- *
- *  switch to security_state_next
- *    if (FPU registers read != last filled values)
- *       break loop; return TC_FAIL
- *
- *    Fill FPU registers with new random values for the next comparison.
- */
-test_result_t host_realm_fpu_access_in_rl_ns_se(void)
+/* Generate random values and write it to SVE Z, P and FFR registers */
+static void ns_sve_write_rand(void)
 {
-	security_state_t sec_state;
-	bool is_sp_present;
-	test_result_t res;
+	bool has_ffr = true;
 
-	/* Verify RME is present and RMM is not TRP */
-	SKIP_TEST_IF_RME_NOT_SUPPORTED_OR_RMM_IS_TRP();
-
-	/* Verify that FFA is there and that it has the correct version. */
-	SKIP_TEST_IF_FFA_VERSION_LESS_THAN(1, 1);
-
-	res = init_realm();
-	if (res != TEST_RESULT_SUCCESS) {
-		return res;
+	if (is_feat_sme_supported() && sme_smstat_sm() &&
+	    !sme_feat_fa64_enabled()) {
+		has_ffr = false;
 	}
 
-	/* Fill FPU registers in Non-secure world */
-	fpu_state_write_rand(&ns_fpu_state_write);
-
-	/* Fill FPU registers in Realm world */
-	if (!fpu_fill_rl()) {
-		ERROR("fpu_fill_rl error\n");
-		goto destroy_realm;
+	sve_z_regs_write_rand(&ns_sve_z_regs_write);
+	sve_p_regs_write_rand(&ns_sve_p_regs_write);
+	if (has_ffr) {
+		sve_ffr_regs_write_rand(&ns_sve_ffr_regs_write);
 	}
-	sec_state = REALM_WORLD;
+}
 
-	/* Fill FPU registers in Secure world if present */
-	res = init_sp();
-	if (res == TEST_RESULT_SUCCESS) {
-		if (!fpu_fill_sec()) {
-			ERROR("fpu_fill_sec error\n");
-			goto destroy_realm;
+/* Read SVE Z, P and FFR registers and compare it with the last written values */
+static test_result_t ns_sve_read_and_compare(void)
+{
+	test_result_t rc = TEST_RESULT_SUCCESS;
+	uint64_t bitmap;
+	bool has_ffr = true;
+
+	if (is_feat_sme_supported() && sme_smstat_sm() &&
+	    !sme_feat_fa64_enabled()) {
+		has_ffr = false;
+	}
+
+	/* Clear old state */
+	memset((void *)&ns_sve_z_regs_read, 0, sizeof(ns_sve_z_regs_read));
+	memset((void *)&ns_sve_p_regs_read, 0, sizeof(ns_sve_p_regs_read));
+	memset((void *)&ns_sve_ffr_regs_read, 0, sizeof(ns_sve_ffr_regs_read));
+
+	/* Read Z, P, FFR registers to compare it with the last written values */
+	sve_z_regs_read(&ns_sve_z_regs_read);
+	sve_p_regs_read(&ns_sve_p_regs_read);
+	if (has_ffr) {
+		sve_ffr_regs_read(&ns_sve_ffr_regs_read);
+	}
+
+	bitmap = sve_z_regs_compare(&ns_sve_z_regs_write, &ns_sve_z_regs_read);
+	if (bitmap != 0UL) {
+		ERROR("SVE Z regs compare failed (bitmap: 0x%016llx)\n",
+		      bitmap);
+		rc = TEST_RESULT_FAIL;
+	}
+
+	bitmap = sve_p_regs_compare(&ns_sve_p_regs_write, &ns_sve_p_regs_read);
+	if (bitmap != 0UL) {
+		ERROR("SVE P regs compare failed (bitmap: 0x%016llx)\n",
+		      bitmap);
+		rc = TEST_RESULT_FAIL;
+	}
+
+	if (has_ffr) {
+		bitmap = sve_ffr_regs_compare(&ns_sve_ffr_regs_write,
+					      &ns_sve_ffr_regs_read);
+		if (bitmap != 0) {
+			ERROR("SVE FFR regs compare failed "
+			      "(bitmap: 0x%016llx)\n", bitmap);
+			rc = TEST_RESULT_FAIL;
+		}
+	}
+
+	return rc;
+}
+
+/*
+ * Generate random values and write it to Streaming SVE Z, P and FFR registers.
+ */
+static void ns_sme_write_rand(void)
+{
+	/*
+	 * TODO: more SME specific registers like ZA, ZT0 can be included later.
+	 */
+
+	/* Fill SVE registers in normal or streaming SVE mode */
+	ns_sve_write_rand();
+}
+
+/*
+ * Read streaming SVE Z, P and FFR registers and compare it with the last
+ * written values
+ */
+static test_result_t ns_sme_read_and_compare(void)
+{
+	/*
+	 * TODO: more SME specific registers like ZA, ZT0 can be included later.
+	 */
+
+	/* Compares SVE registers in normal or streaming SVE mode */
+	return ns_sve_read_and_compare();
+}
+
+static char *simd_type_to_str(simd_test_t type)
+{
+	if (type == TEST_FPU) {
+		return "FPU";
+	} else if (type == TEST_SVE) {
+		return "SVE";
+	} else if (type == TEST_SME) {
+		return "SME";
+	} else {
+		return "UNKNOWN";
+	}
+}
+
+static void ns_simd_print_cmd_config(bool cmd, simd_test_t type)
+{
+	char __unused *tstr = simd_type_to_str(type);
+	char __unused *cstr = cmd ? "write rand" : "read and compare";
+
+	if (type == TEST_SME) {
+		if (sme_smstat_sm()) {
+			INFO("TFTF: NS [%s] %s. Config: smcr: 0x%llx, SM: on\n",
+			     tstr, cstr, (uint64_t)read_smcr_el2());
+		} else {
+			INFO("TFTF: NS [%s] %s. Config: smcr: 0x%llx, "
+			     "zcr: 0x%llx sve_hint: %d SM: off\n", tstr, cstr,
+			     (uint64_t)read_smcr_el2(),
+			     (uint64_t)sve_read_zcr_elx(),
+			     tftf_smc_get_sve_hint());
+		}
+	} else if (type == TEST_SVE) {
+		INFO("TFTF: NS [%s] %s. Config: zcr: 0x%llx, sve_hint: %d\n",
+		     tstr, cstr, (uint64_t)sve_read_zcr_elx(),
+		     tftf_smc_get_sve_hint());
+	} else {
+		INFO("TFTF: NS [%s] %s\n", tstr, cstr);
+	}
+}
+
+/*
+ * Randomly select TEST_SME or TEST_FPU. For TEST_SME, randomly select below
+ * configurations:
+ * - enable/disable streaming mode
+ *   For streaming mode:
+ *   - enable or disable FA64
+ *   - select random streaming vector length
+ *   For normal SVE mode:
+ *   - select random normal SVE vector length
+ */
+static simd_test_t ns_sme_select_random_config(void)
+{
+	simd_test_t type;
+	static unsigned int counter;
+
+	/* Use a static counter to mostly select TEST_SME case. */
+	if ((counter % 8U) != 0) {
+		/* Use counter to toggle between Streaming mode on or off */
+		if (is_armv8_2_sve_present() && ((counter % 2U) != 0)) {
+			sme_smstop(SMSTOP_SM);
+			sve_config_vq(SVE_GET_RANDOM_VQ);
+
+			if ((counter % 3U) != 0) {
+				tftf_smc_set_sve_hint(true);
+			} else {
+				tftf_smc_set_sve_hint(false);
+			}
+		} else {
+			sme_smstart(SMSTART_SM);
+			sme_config_svq(SME_GET_RANDOM_SVQ);
+
+			if ((counter % 3U) != 0) {
+				sme_enable_fa64();
+			} else {
+				sme_disable_fa64();
+			}
+		}
+		type = TEST_SME;
+	} else {
+		type = TEST_FPU;
+	}
+	counter++;
+
+	return type;
+}
+
+/*
+ * Randomly select TEST_SVE or TEST_FPU. For TEST_SVE, configure zcr_el2 with
+ * random vector length and randomly enable or disable SMC SVE hint bit.
+ */
+static simd_test_t ns_sve_select_random_config(void)
+{
+	simd_test_t type;
+	static unsigned int counter;
+
+	/* Use a static counter to mostly select TEST_SVE case. */
+	if ((counter % 4U) != 0) {
+		sve_config_vq(SVE_GET_RANDOM_VQ);
+
+		if ((counter % 2U) != 0) {
+			tftf_smc_set_sve_hint(true);
+		} else {
+			tftf_smc_set_sve_hint(false);
 		}
 
-		sec_state = SECURE_WORLD;
-		is_sp_present = true;
+		type = TEST_SVE;
 	} else {
-		is_sp_present = false;
+		type = TEST_FPU;
+	}
+	counter++;
+
+	return type;
+}
+
+/*
+ * Configure NS world SIMD. Randomly choose to test SVE or FPU registers if
+ * system supports SVE.
+ *
+ * Returns either TEST_FPU or TEST_SVE or TEST_SME
+ */
+static simd_test_t ns_simd_select_random_config(void)
+{
+	simd_test_t type;
+
+	/* cleanup old config for SME */
+	if (is_feat_sme_supported()) {
+		sme_smstop(SMSTOP_SM);
+		sme_enable_fa64();
 	}
 
-	for (uint32_t i = 0; i < 128; i++) {
-		sec_state = get_random_security_state(sec_state, is_sp_present);
+	/* Cleanup old config for SVE */
+	if (is_armv8_2_sve_present()) {
+		tftf_smc_set_sve_hint(false);
+	}
+
+	if (is_armv8_2_sve_present() && is_feat_sme_supported()) {
+		if (rand() % 2) {
+			type = ns_sme_select_random_config();
+		} else {
+			type = ns_sve_select_random_config();
+		}
+	} else if (is_feat_sme_supported()) {
+		type = ns_sme_select_random_config();
+	} else if (is_armv8_2_sve_present()) {
+		type = ns_sve_select_random_config();
+	} else {
+		type = TEST_FPU;
+	}
+
+	return type;
+}
+
+/* Select random NS SIMD config and write random values to its registers */
+static simd_test_t ns_simd_write_rand(void)
+{
+	simd_test_t type;
+
+	type = ns_simd_select_random_config();
+
+	ns_simd_print_cmd_config(true, type);
+
+	if (type == TEST_SME) {
+		ns_sme_write_rand();
+	} else if (type == TEST_SVE) {
+		ns_sve_write_rand();
+	} else {
+		fpu_q_regs_write_rand(ns_fpu_q_regs_write);
+	}
+
+	/* fpcr, fpsr common to all configs */
+	fpu_cs_regs_write_rand(&ns_fpu_cs_regs_write);
+
+	return type;
+}
+
+/* Read and compare the NS SIMD registers with the last written values */
+static test_result_t ns_simd_read_and_compare(simd_test_t type)
+{
+	test_result_t rc = TEST_RESULT_SUCCESS;
+
+	ns_simd_print_cmd_config(false, type);
+
+	if (type == TEST_SME) {
+		rc = ns_sme_read_and_compare();
+	} else if (type == TEST_SVE) {
+		rc = ns_sve_read_and_compare();
+	} else {
+		fpu_q_regs_read(ns_fpu_q_regs_read);
+		if (fpu_q_regs_compare(ns_fpu_q_regs_write,
+				       ns_fpu_q_regs_read)) {
+			ERROR("FPU Q registers compare failed\n");
+			rc = TEST_RESULT_FAIL;
+		}
+	}
+
+	/* fpcr, fpsr common to all configs */
+	fpu_cs_regs_read(&ns_fpu_cs_regs_read);
+	if (fpu_cs_regs_compare(&ns_fpu_cs_regs_write, &ns_fpu_cs_regs_read)) {
+		ERROR("FPCR/FPSR registers compare failed\n");
+		rc = TEST_RESULT_FAIL;
+	}
+
+	return rc;
+}
+
+/* Select random Realm SIMD config and write random values to its registers */
+static simd_test_t rl_simd_write_rand(struct realm *realm, bool rl_sve_en)
+{
+	enum realm_cmd rl_fill_cmd;
+	simd_test_t type;
+	bool __unused rc;
+
+	/* Select random commands to test. SVE or FPU registers in Realm */
+	if (rl_sve_en && (rand() % 2)) {
+		type = TEST_SVE;
+	} else {
+		type = TEST_FPU;
+	}
+
+	INFO("TFTF: RL [%s] write random\n", simd_type_to_str(type));
+	if (type == TEST_SVE) {
+		rl_fill_cmd = REALM_SVE_FILL_REGS;
+	} else {
+		rl_fill_cmd = REALM_REQ_FPU_FILL_CMD;
+	}
+
+	rc = host_enter_realm_execute(realm, rl_fill_cmd, RMI_EXIT_HOST_CALL, 0U);
+	assert(rc);
+
+	return type;
+}
+
+/* Read and compare the Realm SIMD registers with the last written values */
+static bool rl_simd_read_and_compare(struct realm *realm, simd_test_t type)
+{
+	enum realm_cmd rl_cmp_cmd;
+
+	INFO("TFTF: RL [%s] read and compare\n", simd_type_to_str(type));
+	if (type == TEST_SVE) {
+		rl_cmp_cmd = REALM_SVE_CMP_REGS;
+	} else {
+		rl_cmp_cmd = REALM_REQ_FPU_CMP_CMD;
+	}
+
+	return host_enter_realm_execute(realm, rl_cmp_cmd, RMI_EXIT_HOST_CALL,
+					0U);
+}
+
+/* Send request to SP to fill FPU/SIMD regs with secure template values */
+static bool sp_fpu_write_rand(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 sp_fpu_read_and_compare(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;
+}
+
+/*
+ * This test case verifies whether various SIMD related registers like Q[0-31],
+ * FPCR, FPSR, Z[0-31], P[0-15], FFR are preserved by:
+ *
+ * 1. RMM during world switch between NS world and Realm world.
+ * 2. Hafnium during world switch between NS world and Secure world.
+ *
+ * This testcase runs on below SIMD configs:
+ * - SVE only
+ * - SME only
+ * - with SVE and SME
+ * - without SVE and SME
+ *
+ * This testcase runs on below world configs:
+ * - Secure world only
+ * - Realm world only
+ * - With Secure and Realm world
+ */
+test_result_t host_realm_swd_check_simd(void)
+{
+	test_result_t rc;
+	bool sve_en = false;
+	security_state_t sec_state;
+	simd_test_t ns_simd_type;
+	simd_test_t rl_simd_type = TEST_FPU;
+	unsigned int test_iterations;
+	unsigned int num_simd_types;
+	unsigned int num_simd_configs;
+	bool is_sp_present = false;
+	bool is_rl_present = false;
+
+	/* Init and fill FPU registers in Secure world if present */
+	if (init_secure_partition() == TEST_RESULT_SUCCESS) {
+		is_sp_present = true;
+		if (!sp_fpu_write_rand()) {
+			ERROR("%s failed %d\n", __func__, __LINE__);
+			return TEST_RESULT_FAIL;
+		}
+	}
+
+	/* Init and fill FPU or SVE registers in Realm if present */
+	if (init_realm_payload(&realm, &sve_en) == TEST_RESULT_SUCCESS) {
+		is_rl_present = true;
+		rl_simd_type = rl_simd_write_rand(&realm, sve_en);
+	}
+
+	if (!is_sp_present && !is_rl_present) {
+		INFO("Neither SP nor Realm exists. Skipping test case\n");
+		return TEST_RESULT_SKIPPED;
+	}
+
+	/*
+	 * Randomly select and configure NS simd context to test. And fill it
+	 * with random values.
+	 */
+	ns_simd_type = ns_simd_write_rand();
+	sec_state = NONSECURE_WORLD;
+
+	/*
+	 * Find out test iterations based on if SVE is enabled and the number of
+	 * configurations available in the SVE.
+	 */
+
+	/* FPU is always available */
+	num_simd_types = 1U;
+	num_simd_configs = NUM_FPU_CONFIGS;
+
+	if (is_armv8_2_sve_present()) {
+		num_simd_types += 1;
+		num_simd_configs += NUM_SVE_CONFIGS;
+	}
+
+	if (is_feat_sme_supported()) {
+		num_simd_types += 1;
+		num_simd_configs += NUM_SME_CONFIGS;
+	}
+
+	if (num_simd_configs > 0U) {
+		test_iterations = TEST_ITERATIONS_MIN * num_simd_types *
+			num_simd_configs;
+	} else {
+		test_iterations = TEST_ITERATIONS_MIN * num_simd_types;
+	}
+
+	if (is_sp_present && is_rl_present) {
+		test_iterations *= 2U;
+	}
+
+	/*
+	 * Test loop:
+	 *  security_state = get_random_security_state()
+	 *
+	 *  switch to security_state
+	 *    if (SIMD registers read != last filled values)
+	 *       break loop; return TC_FAIL
+	 *
+	 *    Fill SIMD registers with new random values for the next comparison.
+	 */
+	for (uint32_t i = 0U; i < test_iterations; i++) {
+		sec_state = get_random_security_state(sec_state, is_sp_present,
+						      is_rl_present);
 
 		switch (sec_state) {
 		case NONSECURE_WORLD:
-			/* NS world verify its FPU/SIMD state registers */
-			fpu_state_read(&ns_fpu_state_read);
-			if (fpu_state_compare(&ns_fpu_state_write,
-					      &ns_fpu_state_read)) {
+			/*
+			 * Read NS simd context and compare it with last written
+			 * context.
+			 */
+			rc = ns_simd_read_and_compare(ns_simd_type);
+			if (rc != TEST_RESULT_SUCCESS) {
 				ERROR("%s failed %d\n", __func__, __LINE__);
-				goto destroy_realm;
+				rc = TEST_RESULT_FAIL;
+				goto rm_realm;
 			}
 
-			/* Fill FPU state with new random values in NS world */
-			fpu_state_write_rand(&ns_fpu_state_write);
+			/*
+			 * Randomly select and configure NS simd context. And
+			 * fill it with random values for the next compare.
+			 */
+			ns_simd_type = ns_simd_write_rand();
 			break;
 		case REALM_WORLD:
-			/* Realm world verify its FPU/SIMD state registers */
-			if (!fpu_cmp_rl()) {
-				goto destroy_realm;
+			/*
+			 * Enter Realm and read the simd context and compare it
+			 * with last written context.
+			 */
+			if (!rl_simd_read_and_compare(&realm, rl_simd_type)) {
+				ERROR("%s failed %d\n", __func__, __LINE__);
+				rc = TEST_RESULT_FAIL;
+				goto rm_realm;
 			}
 
-			/* Fill FPU state with new random values in Realm */
-			if (!fpu_fill_rl()) {
-				goto destroy_realm;
-			}
-
+			/*
+			 * Randomly select and configure Realm simd context to
+			 * test. Enter realm and fill simd context with random
+			 * values for the next compare.
+			 */
+			rl_simd_type = rl_simd_write_rand(&realm, sve_en);
 			break;
 		case SECURE_WORLD:
+			INFO("TFTF: S  [FPU] read and compare\n");
 			/* Secure world verify its FPU/SIMD state registers */
-			if (!fpu_cmp_sec()) {
-				goto destroy_realm;
+			if (!sp_fpu_read_and_compare()) {
+				ERROR("%s failed %d\n", __func__, __LINE__);
+				rc = TEST_RESULT_FAIL;
+				goto rm_realm;
 			}
 
+			INFO("TFTF: S  [FPU] write random\n");
 			/* Fill FPU state with new random values in SP */
-			if (!fpu_fill_sec()) {
-				goto destroy_realm;
-
+			if (!sp_fpu_write_rand()) {
+				rc = TEST_RESULT_FAIL;
+				goto rm_realm;
 			}
-
 			break;
 		default:
 			break;
 		}
 	}
 
-	if (!host_destroy_realm(&realm)) {
-		ERROR("host_destroy_realm error\n");
+	rc = TEST_RESULT_SUCCESS;
+rm_realm:
+	/* Cleanup old config */
+	if (is_feat_sme_supported()) {
+		sme_smstop(SMSTOP_SM);
+		sme_enable_fa64();
+	}
+
+	/* Cleanup old config */
+	if (is_armv8_2_sve_present()) {
+		tftf_smc_set_sve_hint(false);
+	}
+
+	if (is_rl_present && !host_destroy_realm(&realm)) {
 		return TEST_RESULT_FAIL;
 	}
-	return TEST_RESULT_SUCCESS;
-destroy_realm:
-	host_destroy_realm(&realm);
-	return TEST_RESULT_FAIL;
+
+	return rc;
 }
diff --git a/tftf/tests/runtime_services/secure_service/test_spm_smmu.c b/tftf/tests/runtime_services/secure_service/test_spm_smmu.c
index 6237eb8..f76d6ac 100644
--- a/tftf/tests/runtime_services/secure_service/test_spm_smmu.c
+++ b/tftf/tests/runtime_services/secure_service/test_spm_smmu.c
@@ -7,7 +7,7 @@
 #include <cactus_test_cmds.h>
 #include <debug.h>
 #include <ffa_endpoints.h>
-#include <runtime_services/host_realm_managment/host_realm_rmi.h>
+#include <host_realm_rmi.h>
 #include <smccc.h>
 #include <spm_test_helpers.h>
 #include <test_helpers.h>
diff --git a/tftf/tests/tests-realm-payload.mk b/tftf/tests/tests-realm-payload.mk
index 4d92552..ae4b20a 100644
--- a/tftf/tests/tests-realm-payload.mk
+++ b/tftf/tests/tests-realm-payload.mk
@@ -12,6 +12,7 @@
 	$(addprefix tftf/tests/runtime_services/realm_payload/,		\
 		host_realm_payload_multiple_rec_tests.c			\
 		host_realm_payload_tests.c				\
+		host_realm_simd_common.c				\
 		host_realm_spm.c					\
 		host_realm_payload_simd_tests.c				\
 		host_realm_lpa2_tests.c					\
diff --git a/tftf/tests/tests-realm-payload.xml b/tftf/tests/tests-realm-payload.xml
index a4c12ad..f359e72 100644
--- a/tftf/tests/tests-realm-payload.xml
+++ b/tftf/tests/tests-realm-payload.xml
@@ -72,8 +72,6 @@
 	  function="host_realm_pmuv3_mul_rec" />
 	  <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" />
 	  <testcase name="Realm request set_ripas"
 	  function="host_realm_set_ripas" />
 	  <testcase name="Realm reject set_ripas"
@@ -99,8 +97,8 @@
 	  function="host_sve_realm_check_vectors_leaked" />
 	  <testcase name="Check if Realm gets undefined abort if it access SVE"
 	  function="host_non_sve_realm_check_undef_abort" />
-	  <testcase name="Check various SIMD state preserved across NS/RL switch"
-	  function="host_and_realm_check_simd" />
+	  <testcase name="Check various SIMD state preserved across NS/RL/S switch"
+	  function="host_realm_swd_check_simd" />
 	  <!-- Test Realm for SME -->
 	  <testcase name="Create Realm and test SME ID registers"
 	  function="host_realm_check_sme_id_registers" />