diff --git a/include/runtime_services/spm_common.h b/include/runtime_services/spm_common.h
index f264af9..b090002 100644
--- a/include/runtime_services/spm_common.h
+++ b/include/runtime_services/spm_common.h
@@ -31,7 +31,6 @@
 	unsigned int feature;
 	unsigned int expected_ret;
 };
-unsigned int get_ffa_feature_test_target(const struct ffa_features_test **test_target);
 
 struct mailbox_buffers {
 	void *recv;
@@ -59,6 +58,28 @@
 			);							\
 	} while (false)
 
+/*
+ * Vector length:
+ * SIMD: 128 bits = 16 bytes
+ */
+#define SIMD_VECTOR_LEN_BYTES		16
+#define SIMD_NUM_VECTORS		32
+typedef uint8_t simd_vector_t[SIMD_VECTOR_LEN_BYTES];
+
+/*
+ * Fills SIMD registers with the content of the container v.
+ * Number of vectors is assumed to be SIMD_NUM_VECTORS.
+ */
+void fill_simd_vector_regs(const simd_vector_t v[SIMD_NUM_VECTORS]);
+
+/*
+ * Reads contents of SIMD registers into the provided container v.
+ * Number of vectors is assumed to be SIMD_NUM_VECTORS.
+ */
+void read_simd_vector_regs(simd_vector_t v[SIMD_NUM_VECTORS]);
+
 bool check_spmc_execution_level(void);
 
+unsigned int get_ffa_feature_test_target(const struct ffa_features_test **test_target);
+
 #endif /* SPM_COMMON_H */
diff --git a/spm/cactus/aarch64/cactus_entrypoint.S b/spm/cactus/aarch64/cactus_entrypoint.S
index 383c319..b0f89d4 100644
--- a/spm/cactus/aarch64/cactus_entrypoint.S
+++ b/spm/cactus/aarch64/cactus_entrypoint.S
@@ -1,9 +1,10 @@
 /*
- * Copyright (c) 2017-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2017-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <arch.h>
 #include <asm_macros.S>
 #include <cactus_def.h>
 #include <platform_def.h>
@@ -27,6 +28,16 @@
 	msr	sctlr_el1, x0
 	isb
 
+	/*
+	 * Set CPACR_EL1.FPEN=11 no EL1/0 trapping of
+	 * SVE/Adv. SIMD/FP instructions.
+	 */
+	mov	x1, CPACR_EL1_FPEN(CPACR_EL1_FP_TRAP_NONE)
+	mrs	x0, cpacr_el1
+	orr	x0, x0, x1
+	msr	cpacr_el1, x0
+	isb
+
 	/* Relocate symbols */
 pie_fixup:
 	ldr	x0, =pie_fixup
diff --git a/spm/cactus/cactus_ffa_tests.c b/spm/cactus/cactus_ffa_tests.c
index a5acc56..8e9605d 100644
--- a/spm/cactus/cactus_ffa_tests.c
+++ b/spm/cactus/cactus_ffa_tests.c
@@ -26,6 +26,22 @@
 static const uint32_t null_uuid[4] = {0};
 
 /*
+ * Fill SIMD vectors from secure world side with a unique value.
+ * 0x22 is just a dummy value to be distinguished from the value
+ * in the normal world.
+ */
+void fill_simd_vectors(void)
+{
+	simd_vector_t simd_vectors[SIMD_NUM_VECTORS];
+
+	for (unsigned int num = 0U; num < SIMD_NUM_VECTORS; num++) {
+		memset(simd_vectors[num], 0x22 * num, sizeof(simd_vector_t));
+	}
+
+	fill_simd_vector_regs(simd_vectors);
+}
+
+/*
  * Test FFA_FEATURES interface.
  */
 static void ffa_features_test(void)
diff --git a/spm/cactus/cactus_main.c b/spm/cactus/cactus_main.c
index 1d81988..87ef837 100644
--- a/spm/cactus/cactus_main.c
+++ b/spm/cactus/cactus_main.c
@@ -282,6 +282,10 @@
 			ffa_ret = cactus_error_resp(vm_id, source);
 			break;
 		}
+		case CACTUS_REQ_SIMD_FILL_CMD:
+			fill_simd_vectors();
+			ffa_ret = cactus_success_resp(vm_id, source);
+			break;
 		default:
 			/*
 			 * Currently direct message test is handled here.
diff --git a/spm/cactus/cactus_test_cmds.h b/spm/cactus/cactus_test_cmds.h
index 0db523e..c662d27 100644
--- a/spm/cactus/cactus_test_cmds.h
+++ b/spm/cactus/cactus_test_cmds.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -175,6 +175,21 @@
 }
 
 /**
+ * Request to fill SIMD vectors with dummy values with purpose to check a
+ * save/restore routine during the context switches between secure world and
+ * normal world.
+ *
+ * The command id is the hex representation of the string "SIMD"
+ */
+#define CACTUS_REQ_SIMD_FILL_CMD U(0x53494d44)
+
+static inline smc_ret_values cactus_req_simd_fill_send_cmd(
+	ffa_vm_id_t source, ffa_vm_id_t dest)
+{
+	return cactus_send_cmd(source, dest, CACTUS_REQ_SIMD_FILL_CMD, 0, 0, 0, 0);
+}
+
+/**
  * Template for responses to CACTUS commands.
  */
 static inline smc_ret_values cactus_response(
diff --git a/spm/cactus/cactus_tests.h b/spm/cactus/cactus_tests.h
index 8e838cc..c0c235e 100644
--- a/spm/cactus/cactus_tests.h
+++ b/spm/cactus/cactus_tests.h
@@ -14,6 +14,12 @@
  */
 
 /*
+ * Alter SIMD vectors to check saving of the context while switching between
+ * the normal world and the secure world.
+ */
+void fill_simd_vectors(void);
+
+/*
  * Test to FFA interfaces.
  */
 void ffa_memory_management_test(struct mailbox_buffers *mb, ffa_vm_id_t vm_id,
diff --git a/tftf/tests/runtime_services/secure_service/spm_common.c b/tftf/tests/runtime_services/secure_service/spm_common.c
index 74a19a6..cc3ed5d 100644
--- a/tftf/tests/runtime_services/secure_service/spm_common.c
+++ b/tftf/tests/runtime_services/secure_service/spm_common.c
@@ -7,6 +7,62 @@
 #include <ffa_endpoints.h>
 #include <spm_common.h>
 
+#define __STR(x) #x
+#define STR(x) __STR(x)
+#define SIMD_TWO_VECTORS_BYTES_STR	(2 * SIMD_VECTOR_LEN_BYTES)
+
+void fill_simd_vector_regs(const simd_vector_t v[SIMD_NUM_VECTORS])
+{
+#ifdef __aarch64__
+	__asm__ volatile(
+		"ldp q0, q1, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q2, q3, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q4, q5, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q6, q7, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q8, q9, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q10, q11, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q12, q13, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q14, q15, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q16, q17, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q18, q19, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q20, q21, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q22, q23, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q24, q25, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q26, q27, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q28, q29, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"ldp q30, q31, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"sub %0, %0, #" STR(SIMD_NUM_VECTORS * SIMD_VECTOR_LEN_BYTES) ";"
+		: : "r" (v));
+#endif
+}
+
+void read_simd_vector_regs(simd_vector_t v[SIMD_NUM_VECTORS])
+{
+#ifdef __aarch64__
+	memset(v, 0, sizeof(simd_vector_t) * SIMD_NUM_VECTORS);
+
+	__asm__ volatile(
+		"stp q0, q1, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q2, q3, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q4, q5, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q6, q7, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q8, q9, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q10, q11, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q12, q13, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q14, q15, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q16, q17, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q18, q19, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q20, q21, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q22, q23, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q24, q25, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q26, q27, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q28, q29, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"stp q30, q31, [%0], #" STR(SIMD_TWO_VECTORS_BYTES_STR) ";"
+		"sub %0, %0, #" STR(SIMD_NUM_VECTORS * SIMD_VECTOR_LEN_BYTES) ";"
+		: : "r" (v));
+#endif
+}
+
 /*
  * check_spmc_execution_level
  *
diff --git a/tftf/tests/runtime_services/secure_service/test_spm_cpu_features.c b/tftf/tests/runtime_services/secure_service/test_spm_cpu_features.c
new file mode 100644
index 0000000..3b3edba
--- /dev/null
+++ b/tftf/tests/runtime_services/secure_service/test_spm_cpu_features.c
@@ -0,0 +1,70 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <cactus_test_cmds.h>
+#include <ffa_endpoints.h>
+#include <ffa_helpers.h>
+#include <test_helpers.h>
+
+#define SENDER HYP_ID
+#define RECEIVER SP_ID(1)
+
+static const struct ffa_uuid expected_sp_uuids[] = { {PRIMARY_UUID} };
+
+static test_result_t simd_vector_compare(simd_vector_t a[SIMD_NUM_VECTORS],
+					 simd_vector_t b[SIMD_NUM_VECTORS])
+{
+	for (unsigned int num = 0U; num < SIMD_NUM_VECTORS; num++) {
+		if (memcmp(a[num], b[num], sizeof(simd_vector_t)) != 0) {
+			ERROR("Vectors not equal: a:0x%llx b:0x%llx\n",
+				(uint64_t)a[num][0], (uint64_t)b[num][0]);
+			return TEST_RESULT_FAIL;
+		}
+	}
+	return TEST_RESULT_SUCCESS;
+}
+
+/*
+ * Tests that SIMD vectors are preserved during the context switches between
+ * normal world and the secure world.
+ * Fills the SIMD vectors with known values, requests SP to fill the vectors
+ * with a different values, checks that the context is restored on return.
+ */
+test_result_t test_simd_vectors_preserved(void)
+{
+	SKIP_TEST_IF_AARCH32();
+
+	/**********************************************************************
+	 * Verify that FFA is there and that it has the correct version.
+	 **********************************************************************/
+	CHECK_SPMC_TESTING_SETUP(1, 0, expected_sp_uuids);
+
+	simd_vector_t simd_vectors_send[SIMD_NUM_VECTORS],
+		      simd_vectors_receive[SIMD_NUM_VECTORS];
+
+	/* 0x11 is just a dummy value to be distinguished from the value in the
+	 * secure world. */
+	for (unsigned int num = 0U; num < SIMD_NUM_VECTORS; num++) {
+		memset(simd_vectors_send[num], 0x11 * num, sizeof(simd_vector_t));
+	}
+
+	fill_simd_vector_regs(simd_vectors_send);
+
+	smc_ret_values ret = cactus_req_simd_fill_send_cmd(SENDER, RECEIVER);
+
+	if (ret.ret0 != FFA_MSG_SEND_DIRECT_RESP_SMC32) {
+		ERROR("Failed to send message. error: %lx\n", ret.ret2);
+		return TEST_RESULT_FAIL;
+	}
+
+	if (cactus_get_response(ret) == CACTUS_ERROR) {
+		return TEST_RESULT_FAIL;
+	}
+
+	read_simd_vector_regs(simd_vectors_receive);
+
+	return simd_vector_compare(simd_vectors_send, simd_vectors_receive);
+}
diff --git a/tftf/tests/tests-spm.mk b/tftf/tests/tests-spm.mk
index 2a94983..73d2baf 100644
--- a/tftf/tests/tests-spm.mk
+++ b/tftf/tests/tests-spm.mk
@@ -13,4 +13,5 @@
 		test_ffa_memory_sharing.c				\
 		test_ffa_rxtx_map.c					\
 		test_ffa_version.c					\
+		test_spm_cpu_features.c					\
 	)
diff --git a/tftf/tests/tests-spm.xml b/tftf/tests/tests-spm.xml
index 2637acd..5e80988 100644
--- a/tftf/tests/tests-spm.xml
+++ b/tftf/tests/tests-spm.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0" encoding="utf-8"?>
 
 <!--
-  Copyright (c) 2018-2020, Arm Limited. All rights reserved.
+  Copyright (c) 2018-2021, Arm Limited. All rights reserved.
 
   SPDX-License-Identifier: BSD-3-Clause
 -->
@@ -66,4 +66,10 @@
                function="test_ffa_features" />
   </testsuite>
 
+  <testsuite name="SIMD,SVE Registers context"
+             description="Validate context switch between NWd and SWd" >
+     <testcase name="Check that SIMD registers context is preserved"
+               function="test_simd_vectors_preserved" />
+  </testsuite>
+
 </testsuites>