Merge changes from topic "tc0_tfa_v25"

* changes:
  plat: tc0: enable managed exit for primary cactus
  plat: tc0: set PLAT_CACTUS_CORE_COUNT, disable SMMUv3 tests
  plat: tc0: Update GICR base address
diff --git a/Makefile b/Makefile
index 8eba940..3c1cf53 100644
--- a/Makefile
+++ b/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2018-2020, Arm Limited. All rights reserved.
+# Copyright (c) 2018-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -108,16 +108,16 @@
 include fwu/ns_bl1u/ns_bl1u.mk
 include fwu/ns_bl2u/ns_bl2u.mk
 
-# Only platform fvp supports cactus_mm, ivy, quark
+# Only platform fvp supports cactus_mm, quark
 ifeq (${ARCH}-${PLAT},aarch64-fvp)
 include spm/cactus_mm/cactus_mm.mk
-include spm/ivy/ivy.mk
 include spm/quark/quark.mk
 endif
 
-# cactus is supported on platforms: fvp, tc0
+# cactus and ivy are supported on platforms: fvp, tc0
 ifeq (${ARCH}-${PLAT},$(filter ${ARCH}-${PLAT},aarch64-fvp aarch64-tc0))
 include spm/cactus/cactus.mk
+include spm/ivy/ivy.mk
 endif
 
 ################################################################################
@@ -289,9 +289,9 @@
 
 IVY_SOURCES		+= ${LIBC_SRCS}
 IVY_INCLUDES		+= ${PLAT_INCLUDES}
-IVY_CFLAGS		+= ${COMMON_CFLAGS}
+IVY_CFLAGS		+= ${COMMON_CFLAGS} -fpie
 IVY_ASFLAGS		+= ${COMMON_ASFLAGS}
-IVY_LDFLAGS		+= ${COMMON_LDFLAGS}
+IVY_LDFLAGS		+= ${COMMON_LDFLAGS} $(PIE_LDFLAGS)
 
 QUARK_SOURCES		+= ${LIBC_SRCS}
 QUARK_INCLUDES		+= ${PLAT_INCLUDES}
@@ -367,11 +367,6 @@
 	@echo "ERROR: $@ is supported only on AArch64 FVP."
 	@exit 1
 
-.PHONY: ivy
-ivy:
-	@echo "ERROR: $@ is supported only on AArch64 FVP."
-	@exit 1
-
 .PHONY: quark
 quark:
 	@echo "ERROR: $@ is supported only on AArch64 FVP."
@@ -383,6 +378,11 @@
 cactus:
 	@echo "ERROR: $@ is supported only on AArch64 FVP or TC0."
 	@exit 1
+
+.PHONY: ivy
+ivy:
+	@echo "ERROR: $@ is supported only on AArch64 FVP or TC0."
+	@exit 1
 endif
 
 MAKE_DEP = -Wp,-MD,$(DEP) -MT $$@
@@ -524,6 +524,7 @@
 
 ifeq (${ARCH}-${PLAT},aarch64-tc0)
   $(eval $(call MAKE_IMG,cactus))
+  $(eval $(call MAKE_IMG,ivy))
 endif
 
 # The EL3 test payload is only supported in AArch64. It has an independent build
diff --git a/include/common/aarch64/asm_macros.S b/include/common/aarch64/asm_macros.S
index d829133..8a69c38 100644
--- a/include/common/aarch64/asm_macros.S
+++ b/include/common/aarch64/asm_macros.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -79,6 +79,15 @@
 	.endm
 
 	/*
+	 * Create a vector entry that just spins making the exception unrecoverable.
+	 */
+	.macro vector_entry_spin name
+	vector_entry \name
+	b \name
+	end_vector_entry \name
+	.endm
+
+	/*
 	 * This macro calculates the base address of an MP stack using the
 	 * platform_get_core_pos() index, the name of the stack storage and
 	 * the size of each stack
diff --git a/include/common/test_helpers.h b/include/common/test_helpers.h
index 3ee2b53..9c5f6e4 100644
--- a/include/common/test_helpers.h
+++ b/include/common/test_helpers.h
@@ -107,6 +107,14 @@
 		}								\
 	} while (0)
 
+#define SKIP_TEST_IF_SVE_NOT_SUPPORTED()					\
+	do {									\
+		if (!is_armv8_2_sve_present()) {				\
+			tftf_testcase_printf("SVE not supported\n");		\
+			return TEST_RESULT_SKIPPED;				\
+		}								\
+	} while (0)
+
 #define SKIP_TEST_IF_ECV_NOT_SELF_SYNC()					\
 	do {									\
 		if (get_armv8_6_ecv_support() !=				\
@@ -228,6 +236,7 @@
 
 #define CHECK_SPMC_TESTING_SETUP(ffa_major, ffa_minor, expected_uuids)		\
 	do {									\
+		SKIP_TEST_IF_AARCH32();						\
 		const size_t expected_uuids_size =				\
 			 sizeof(expected_uuids) / sizeof(struct ffa_uuid);	\
 		test_result_t ret = check_spmc_testing_set_up(			\
diff --git a/include/lib/extensions/sve.h b/include/lib/extensions/sve.h
new file mode 100644
index 0000000..278dcf3
--- /dev/null
+++ b/include/lib/extensions/sve.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SVE_H
+#define SVE_H
+
+#define fill_sve_helper(num) "ldr z"#num", [%0, #"#num", MUL VL];"
+#define read_sve_helper(num) "str z"#num", [%0, #"#num", MUL VL];"
+
+#endif /* SVE_H */
\ No newline at end of file
diff --git a/include/runtime_services/spm_common.h b/include/runtime_services/spm_common.h
index 685e732..5ccf395 100644
--- a/include/runtime_services/spm_common.h
+++ b/include/runtime_services/spm_common.h
@@ -83,22 +83,29 @@
 /*
  * Vector length:
  * SIMD: 128 bits = 16 bytes
+ * SVE:	 512 bits = 64 bytes
  */
 #define SIMD_VECTOR_LEN_BYTES		16
+#define SVE_VECTOR_LEN_BYTES		64
+
 #define SIMD_NUM_VECTORS		32
+#define SVE_NUM_VECTORS			32
 typedef uint8_t simd_vector_t[SIMD_VECTOR_LEN_BYTES];
+typedef uint8_t sve_vector_t[SVE_VECTOR_LEN_BYTES];
 
 /*
- * Fills SIMD registers with the content of the container v.
- * Number of vectors is assumed to be SIMD_NUM_VECTORS.
+ * Fills SIMD/SVE registers with the content of the container v.
+ * Number of vectors is assumed to be SIMD/SVE_NUM_VECTORS.
  */
 void fill_simd_vector_regs(const simd_vector_t v[SIMD_NUM_VECTORS]);
+void fill_sve_vector_regs(const sve_vector_t v[SVE_NUM_VECTORS]);
 
 /*
- * Reads contents of SIMD registers into the provided container v.
- * Number of vectors is assumed to be SIMD_NUM_VECTORS.
+ * Reads contents of SIMD/SVE registers into the provided container v.
+ * Number of vectors is assumed to be SIMD/SVE_NUM_VECTORS.
  */
 void read_simd_vector_regs(simd_vector_t v[SIMD_NUM_VECTORS]);
+void read_sve_vector_regs(sve_vector_t v[SVE_NUM_VECTORS]);
 
 bool check_spmc_execution_level(void);
 
diff --git a/lib/aarch64/exception_stubs.S b/lib/aarch64/exception_stubs.S
index d418451..b186e82 100644
--- a/lib/aarch64/exception_stubs.S
+++ b/lib/aarch64/exception_stubs.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,26 +12,21 @@
  * Simplistic exceptions vector table.
  * All entries spin, which means all types of exceptions are unrecoverable.
  */
-	.macro vector_entry_spin name
-	vector_entry \name
-	b \name
-	end_vector_entry \name
-	.endm
 
 vector_base exception_stubs
-vector_entry_spin SynchronousExceptionSP0
-vector_entry_spin IrqSP0
-vector_entry_spin FiqSP0
-vector_entry_spin SErrorSP0
-vector_entry_spin SynchronousExceptionSPx
-vector_entry_spin IrqSPx
-vector_entry_spin FiqSPx
-vector_entry_spin SErrorSPx
-vector_entry_spin SynchronousExceptionA64
-vector_entry_spin IrqA64
-vector_entry_spin FiqA64
-vector_entry_spin SErrorA64
-vector_entry_spin SynchronousExceptionA32
-vector_entry_spin IrqA32
-vector_entry_spin FiqA32
-vector_entry_spin SErrorA32
+vector_entry_spin sync_exception_sp_el0
+vector_entry_spin irq_sp_el0
+vector_entry_spin fiq_sp_el0
+vector_entry_spin serror_sp_el0
+vector_entry_spin sync_exception_sp_elx
+vector_entry_spin irq_sp_elx
+vector_entry_spin fiq_sp_elx
+vector_entry_spin serror_sp_elx
+vector_entry_spin sync_exception_aarch64
+vector_entry_spin irq_aarch64
+vector_entry_spin fiq_aarch64
+vector_entry_spin serror_aarch64
+vector_entry_spin sync_exception_aarch32
+vector_entry_spin irq_aarch32
+vector_entry_spin fiq_aarch32
+vector_entry_spin serror_aarch32
diff --git a/plat/arm/fvp/platform.mk b/plat/arm/fvp/platform.mk
index 42779c7..4f8c4bd 100644
--- a/plat/arm/fvp/platform.mk
+++ b/plat/arm/fvp/platform.mk
@@ -83,6 +83,7 @@
 			plat/arm/fvp/plat_setup.c
 
 CACTUS_SOURCES	+=	plat/arm/fvp/${ARCH}/plat_helpers.S
+IVY_SOURCES	+=	plat/arm/fvp/${ARCH}/plat_helpers.S
 
 # Firmware update is implemented on FVP.
 FIRMWARE_UPDATE := 1
diff --git a/plat/arm/tc0/platform.mk b/plat/arm/tc0/platform.mk
index faf0d19..1cfe9ee 100644
--- a/plat/arm/tc0/platform.mk
+++ b/plat/arm/tc0/platform.mk
@@ -32,6 +32,7 @@
 			plat/arm/tc0/tc0_topology.c
 
 CACTUS_SOURCES	+=	plat/arm/tc0/${ARCH}/plat_helpers.S
+IVY_SOURCES	+=	plat/arm/tc0/${ARCH}/plat_helpers.S
 
 PLAT_TESTS_SKIP_LIST	:=	plat/arm/tc0/tests_to_skip.txt
 
diff --git a/spm/cactus/cactus.h b/spm/cactus/cactus.h
index cbf2dcb..c7176c2 100644
--- a/spm/cactus/cactus.h
+++ b/spm/cactus/cactus.h
@@ -26,11 +26,4 @@
 #define CACTUS_BSS_START	((uintptr_t)&__BSS_START__)
 #define CACTUS_BSS_END		((uintptr_t)&__BSS_END__)
 
-enum stdout_route {
-	PL011_AS_STDOUT = 0,
-	HVC_CALL_AS_STDOUT,
-};
-
-void set_putc_impl(enum stdout_route);
-
 #endif /* __CACTUS_H__ */
diff --git a/spm/cactus/cactus.mk b/spm/cactus/cactus.mk
index ae66c1d..5247fde 100644
--- a/spm/cactus/cactus.mk
+++ b/spm/cactus/cactus.mk
@@ -33,14 +33,14 @@
 	$(addprefix spm/cactus/,			\
 		aarch64/cactus_entrypoint.S		\
 		aarch64/cactus_exceptions.S		\
-		cactus_debug.c				\
 		cactus_interrupt.c			\
 		cactus_main.c				\
 	)						\
 	$(addprefix spm/common/,			\
 		aarch64/sp_arch_helpers.S		\
+		sp_debug.c				\
 		sp_helpers.c				\
-		spm_helpers.c					\
+		spm_helpers.c				\
 	)						\
 	$(addprefix spm/cactus/cactus_tests/,		\
 		cactus_message_loop.c			\
diff --git a/spm/cactus/cactus_main.c b/spm/cactus/cactus_main.c
index 73606bd..59cd7e7 100644
--- a/spm/cactus/cactus_main.c
+++ b/spm/cactus/cactus_main.c
@@ -16,9 +16,11 @@
 #include <lib/tftf_lib.h>
 #include <lib/xlat_tables/xlat_mmu_helpers.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
+
 #include <plat_arm.h>
 #include <plat/common/platform.h>
 #include <platform_def.h>
+#include <sp_debug.h>
 #include <sp_helpers.h>
 #include <spm_helpers.h>
 #include <std_svc.h>
diff --git a/spm/cactus/cactus_debug.c b/spm/common/sp_debug.c
similarity index 65%
rename from spm/cactus/cactus_debug.c
rename to spm/common/sp_debug.c
index 30a2527..396253b 100644
--- a/spm/cactus/cactus_debug.c
+++ b/spm/common/sp_debug.c
@@ -1,15 +1,15 @@
 /*
- * Copyright (c) 2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <drivers/arm/pl011.h>
 #include <drivers/console.h>
+#include <sp_debug.h>
+#include <sp_helpers.h>
 #include <spm_helpers.h>
 
-#include "cactus.h"
-
 static int (*putc_impl)(int);
 
 static int putc_hypcall(int c)
@@ -19,6 +19,18 @@
 	return c;
 }
 
+static int putc_svccall(int c)
+{
+	/* TODO svc call */
+	svc_args args = {
+		.fid = SPM_DEBUG_LOG,
+		.arg1 = c
+	};
+	sp_svc(&args);
+
+	return c;
+}
+
 static int putc_uart(int c)
 {
 	console_pl011_putc(c);
@@ -34,6 +46,10 @@
 		putc_impl = putc_hypcall;
 		return;
 
+	case SVC_CALL_AS_STDOUT:
+		putc_impl = putc_svccall;
+		return;
+
 	case PL011_AS_STDOUT:
 	default:
 		break;
diff --git a/spm/common/sp_debug.h b/spm/common/sp_debug.h
new file mode 100644
index 0000000..e35c602
--- /dev/null
+++ b/spm/common/sp_debug.h
@@ -0,0 +1,13 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+enum stdout_route {
+	PL011_AS_STDOUT = 0,
+	HVC_CALL_AS_STDOUT,
+	SVC_CALL_AS_STDOUT,
+};
+
+void set_putc_impl(enum stdout_route);
diff --git a/spm/common/sp_helpers.h b/spm/common/sp_helpers.h
index 399200a..9f5400c 100644
--- a/spm/common/sp_helpers.h
+++ b/spm/common/sp_helpers.h
@@ -9,7 +9,7 @@
 
 #include <stdint.h>
 #include <tftf_lib.h>
-#include <ffa_helpers.h>
+#include <spm_common.h>
 
 typedef struct {
 	u_register_t fid;
diff --git a/spm/ivy/aarch64/ivy_entrypoint.S b/spm/ivy/app/aarch64/ivy_entrypoint.S
similarity index 86%
rename from spm/ivy/aarch64/ivy_entrypoint.S
rename to spm/ivy/app/aarch64/ivy_entrypoint.S
index c6cb8b3..d981d6a 100644
--- a/spm/ivy/aarch64/ivy_entrypoint.S
+++ b/spm/ivy/app/aarch64/ivy_entrypoint.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/spm/ivy/app/ivy.h b/spm/ivy/app/ivy.h
new file mode 100644
index 0000000..a40f7e1
--- /dev/null
+++ b/spm/ivy/app/ivy.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef IVY_H
+#define IVY_H
+
+#include <stdint.h>
+
+/* Linker symbols used to figure out the memory layout of the S-EL1 shim. */
+extern uintptr_t __SHIM_TEXT_START__, __SHIM_TEXT_END__;
+#define SHIM_TEXT_START		((uintptr_t)&__SHIM_TEXT_START__)
+#define SHIM_TEXT_END		((uintptr_t)&__SHIM_TEXT_END__)
+
+extern uintptr_t __SHIM_RODATA_START__, __SHIM_RODATA_END__;
+#define SHIM_RODATA_START	((uintptr_t)&__SHIM_RODATA_START__)
+#define SHIM_RODATA_END		((uintptr_t)&__SHIM_RODATA_END__)
+
+extern uintptr_t __SHIM_DATA_START__, __SHIM_DATA_END__;
+#define SHIM_DATA_START		((uintptr_t)&__SHIM_DATA_START__)
+#define SHIM_DATA_END		((uintptr_t)&__SHIM_DATA_END__)
+
+extern uintptr_t __SHIM_BSS_START__, __SHIM_BSS_END__;
+#define SHIM_BSS_START		((uintptr_t)&__SHIM_BSS_START__)
+#define SHIM_BSS_END		((uintptr_t)&__SHIM_BSS_END__)
+
+/* Linker symbols used to figure out the memory layout of Ivy (S-EL0). */
+extern uintptr_t __TEXT_START__, __TEXT_END__;
+#define IVY_TEXT_START		((uintptr_t)&__TEXT_START__)
+#define IVY_TEXT_END		((uintptr_t)&__TEXT_END__)
+
+extern uintptr_t __RODATA_START__, __RODATA_END__;
+#define IVY_RODATA_START	((uintptr_t)&__RODATA_START__)
+#define IVY_RODATA_END		((uintptr_t)&__RODATA_END__)
+
+extern uintptr_t __DATA_START__, __DATA_END__;
+#define IVY_DATA_START		((uintptr_t)&__DATA_START__)
+#define IVY_DATA_END		((uintptr_t)&__DATA_END__)
+
+extern uintptr_t __BSS_START__, __BSS_END__;
+#define IVY_BSS_START		((uintptr_t)&__BSS_START__)
+#define IVY_BSS_END		((uintptr_t)&__BSS_END__)
+
+#endif /* __IVY_H__ */
diff --git a/spm/ivy/ivy_def.h b/spm/ivy/app/ivy_def.h
similarity index 92%
rename from spm/ivy/ivy_def.h
rename to spm/ivy/app/ivy_def.h
index 729c46d..815a59e 100644
--- a/spm/ivy/ivy_def.h
+++ b/spm/ivy/app/ivy_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,7 +12,7 @@
  */
 
 /* Up to 2 MiB at an arbitrary address that doesn't overlap the devices. */
-#define IVY_IMAGE_BASE			ULL(0x90000000)
+#define IVY_IMAGE_BASE			ULL(0x1000)
 #define IVY_IMAGE_SIZE			ULL(0x200000)
 
 /* Memory reserved for stacks */
diff --git a/spm/ivy/app/ivy_main.c b/spm/ivy/app/ivy_main.c
new file mode 100644
index 0000000..28f93ab
--- /dev/null
+++ b/spm/ivy/app/ivy_main.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <errno.h>
+#include <ffa_helpers.h>
+#include <sp_debug.h>
+#include <sp_helpers.h>
+
+#include "ivy.h"
+
+/* Host machine information injected by the build system in the ELF file. */
+extern const char build_message[];
+extern const char version_string[];
+
+void __dead2 ivy_main(void)
+{
+	u_register_t ret;
+	svc_args args;
+
+	set_putc_impl(SVC_CALL_AS_STDOUT);
+
+	NOTICE("Entering S-EL0 Secure Partition\n");
+	NOTICE("%s\n", build_message);
+	NOTICE("%s\n", version_string);
+
+init:
+	args = (svc_args){.fid = FFA_MSG_WAIT};
+	ret = sp_svc(&args);
+
+	while (1) {
+		if (ret != FFA_MSG_SEND_DIRECT_REQ_SMC32) {
+			ERROR("unknown FF-A request %lx\n", ret);
+			goto init;
+		}
+
+		VERBOSE("Received request: %lx\n", args.arg3);
+
+		args.fid = FFA_MSG_SEND_DIRECT_RESP_SMC32;
+		args.arg1 = 0x80020000;
+		args.arg2 = 0;
+		args.arg3 = 0;
+
+		ret = sp_svc(&args);
+	}
+}
diff --git a/spm/ivy/app/plat/arm/fvp/fdts/ivy.dts b/spm/ivy/app/plat/arm/fvp/fdts/ivy.dts
new file mode 100644
index 0000000..49a84bd
--- /dev/null
+++ b/spm/ivy/app/plat/arm/fvp/fdts/ivy.dts
@@ -0,0 +1,32 @@
+/*
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * This file is a Partition Manifest (PM) for a minimal Secure Partition (SP)
+ * that has additional optional properties defined.
+ */
+
+
+/dts-v1/;
+
+/ {
+	compatible = "arm,ffa-manifest-1.0";
+
+	/* Properties */
+	description = "ivy-1";
+	ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
+	uuid = <0xeaba83d8 0xbaaf4eaf 0x8144f7fd 0xcbe544a7>;
+	execution-ctx-count = <1>;
+	exception-level = <2>; /* S-EL1 */
+	execution-state = <0>; /* AARCH64 */
+	load-address = <0x7600000>;
+	entrypoint-offset = <0x00001000>;
+	xlat-granule = <0>; /* 4KiB */
+	boot-order = <0>;
+	messaging-method = <0>; /* Direct messaging only */
+	run-time-model = <1>; /* SP pre-emptible */
+
+	/* Boot protocol */
+	gp-register-num = <0x0>;
+};
diff --git a/spm/ivy/app/plat/arm/fvp/include/ivy_platform_def.h b/spm/ivy/app/plat/arm/fvp/include/ivy_platform_def.h
new file mode 100644
index 0000000..3658c83
--- /dev/null
+++ b/spm/ivy/app/plat/arm/fvp/include/ivy_platform_def.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#ifndef CACTUS_PLATFORM_DEF_H
+#define CACTUS_PLATFORM_DEF_H
+
+#define PLAT_ARM_DEVICE0_BASE		DEVICE0_BASE
+#define PLAT_ARM_DEVICE0_SIZE		DEVICE0_SIZE
+
+#define CACTUS_PL011_UART_BASE		PL011_UART2_BASE
+#define CACTUS_PL011_UART_CLK_IN_HZ	PL011_UART2_CLK_IN_HZ
+
+#define PLAT_CACTUS_RX_BASE		ULL(0x7300000)
+
+#define CACTUS_PRIMARY_EC_COUNT		(8U)
+#define CACTUS_SECONDARY_EC_COUNT	(8U)
+#define CACTUS_TERTIARY_EC_COUNT	(8U)
+
+#endif /* CACTUS_PLATFORM_DEF_H */
diff --git a/spm/ivy/app/plat/arm/fvp/platform.mk b/spm/ivy/app/plat/arm/fvp/platform.mk
new file mode 100644
index 0000000..1e9a43b
--- /dev/null
+++ b/spm/ivy/app/plat/arm/fvp/platform.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+FVP_IVY_BASE		= spm/ivy/app/plat/arm/fvp
+
+PLAT_INCLUDES		+= -I${FVP_IVY_BASE}/include/
+
+# Add the FDT source
+IVY_DTS		= ${FVP_IVY_BASE}/fdts/ivy.dts
+
+# List of FDTS to copy
+FDTS_CP_LIST		= ${FVP_IVY_BASE}/fdts/ivy.dts
diff --git a/spm/ivy/app/plat/arm/tc0/fdts/ivy.dts b/spm/ivy/app/plat/arm/tc0/fdts/ivy.dts
new file mode 100644
index 0000000..2a22e20
--- /dev/null
+++ b/spm/ivy/app/plat/arm/tc0/fdts/ivy.dts
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * This file is a Partition Manifest (PM) for a minimal Secure Partition (SP)
+ * that has additional optional properties defined.
+ */
+
+
+/dts-v1/;
+
+/ {
+	compatible = "arm,ffa-manifest-1.0";
+
+	/* Properties */
+	description = "ivy-1";
+	ffa-version = <0x00010000>; /* 31:16 - Major, 15:0 - Minor */
+	uuid = <0xeaba83d8 0xbaaf4eaf 0x8144f7fd 0xcbe544a7>;
+	id = <1>;
+	auxiliary-id = <0xae>;
+	stream-endpoint-ids = <0 1 2 3>;
+	execution-ctx-count = <1>;
+	exception-level = <2>; /* S-EL1 */
+	execution-state = <0>; /* AARCH64 */
+	load-address = <0xfe280000>;
+	entrypoint-offset = <0x00001000>;
+	xlat-granule = <0>; /* 4KiB */
+	boot-order = <0>;
+	messaging-method = <0>; /* Direct messaging only */
+	run-time-model = <1>; /* SP pre-emptible */
+
+	/* Boot protocol */
+	gp-register-num = <0x0>;
+};
diff --git a/spm/ivy/app/plat/arm/tc0/include/ivy_platform_def.h b/spm/ivy/app/plat/arm/tc0/include/ivy_platform_def.h
new file mode 100644
index 0000000..3658c83
--- /dev/null
+++ b/spm/ivy/app/plat/arm/tc0/include/ivy_platform_def.h
@@ -0,0 +1,24 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#ifndef CACTUS_PLATFORM_DEF_H
+#define CACTUS_PLATFORM_DEF_H
+
+#define PLAT_ARM_DEVICE0_BASE		DEVICE0_BASE
+#define PLAT_ARM_DEVICE0_SIZE		DEVICE0_SIZE
+
+#define CACTUS_PL011_UART_BASE		PL011_UART2_BASE
+#define CACTUS_PL011_UART_CLK_IN_HZ	PL011_UART2_CLK_IN_HZ
+
+#define PLAT_CACTUS_RX_BASE		ULL(0x7300000)
+
+#define CACTUS_PRIMARY_EC_COUNT		(8U)
+#define CACTUS_SECONDARY_EC_COUNT	(8U)
+#define CACTUS_TERTIARY_EC_COUNT	(8U)
+
+#endif /* CACTUS_PLATFORM_DEF_H */
diff --git a/spm/ivy/app/plat/arm/tc0/platform.mk b/spm/ivy/app/plat/arm/tc0/platform.mk
new file mode 100644
index 0000000..10342d2
--- /dev/null
+++ b/spm/ivy/app/plat/arm/tc0/platform.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2021, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+TC0_IVY_BASE		= spm/ivy/app/plat/arm/tc0
+
+PLAT_INCLUDES		+= -I${TC0_IVY_BASE}/include/
+
+# Add the FDT source
+IVY_DTS		= ${TC0_IVY_BASE}/fdts/ivy.dts
+
+# List of FDTS to copy
+FDTS_CP_LIST		= ${TC0_IVY_BASE}/fdts/ivy.dts
diff --git a/spm/ivy/ivy.dts b/spm/ivy/ivy.dts
deleted file mode 100644
index 4c5a11a..0000000
--- a/spm/ivy/ivy.dts
+++ /dev/null
@@ -1,95 +0,0 @@
-/*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <sp_res_desc_def.h>
-
-#include "ivy_def.h"
-
-/* 4 KiB pages */
-#define PAGE_SIZE			U(0x1000)
-
-/*
- * FVP platform layout. The defines are hardcoded here because including the
- * platform headers have too many dependencies.
- * TODO: Move this to the platform layer.
- */
-#define V2M_IOFPGA_BASE			ULL(0x1c000000)
-#define V2M_IOFPGA_SIZE			ULL(0x03000000)
-
-/* Aggregate of all devices in the first GB. */
-#define DEVICE0_BASE			ULL(0x20000000)
-#define DEVICE0_SIZE			ULL(0x0c200000)
-
-/dts-v1/;
-
-/ {
-	compatible = "arm,sp_rd";
-
-	attribute {
-		version = <0x00000001>;
-		sp_type = <RD_ATTR_TYPE_UP_MIGRATABLE>;
-		pe_mpidr = <0>; /* Unused */
-		runtime_el = <RD_ATTR_RUNTIME_SEL0>;
-		exec_type = <RD_ATTR_RUNTIME>;
-		panic_policy = <RD_ATTR_PANIC_ONESHOT>;
-		xlat_granule = <RD_ATTR_XLAT_GRANULE_4KB>;
-		binary_size = <IVY_IMAGE_SIZE>;
-		load_address = <0x00000000 IVY_IMAGE_BASE>;
-		entrypoint = <0x00000000 IVY_IMAGE_BASE>;
-	};
-
-	memory_regions {
-		v2m_iofpga {
-			str = "V2M IOFPGA";
-			base = <0x00000000 V2M_IOFPGA_BASE>;
-			size = <0x00000000 V2M_IOFPGA_SIZE>;
-			attr = <RD_MEM_DEVICE>;
-		};
-
-		device0 {
-			str = "Device 0";
-			base = <0x00000000 DEVICE0_BASE>;
-			size = <0x00000000 DEVICE0_SIZE>;
-			attr = <RD_MEM_DEVICE>;
-		};
-
-		spm_buffer {
-			str = "SPM buffer";
-			base = <0x00000000 IVY_SPM_BUF_BASE>;
-			size = <0x00000000 IVY_SPM_BUF_SIZE>;
-			attr = <RD_MEM_NORMAL_SPM_SP_SHARED_MEM>;
-		};
-
-		ns_buffer {
-			str = "NS buffer";
-			base = <0x00000000 IVY_NS_BUF_BASE>;
-			size = <0x00000000 IVY_NS_BUF_SIZE>;
-			attr = <RD_MEM_NORMAL_CLIENT_SHARED_MEM>;
-		};
-	};
-
-	notifications {
-		notification_0 {
-			attr = <0>;
-			pe = <0>;
-		};
-	};
-
-	services {
-		test_service_1 {
-			uuid = <IVY_SERVICE1_UUID_RD>;
-
-			accessibility = <(RD_SERV_ACCESS_SECURE |
-					  RD_SERV_ACCESS_EL3 |
-					  RD_SERV_ACCESS_NORMAL)>;
-			request_type = <(RD_SERV_SUPPORT_BLOCKING |
-					 RD_SERV_SUPPORT_NON_BLOCKING)>;
-			connection_quota = <10>;
-			sec_mem_size = <0>;
-			interrupt_num = <0>;
-		};
-	};
-};
diff --git a/spm/ivy/ivy.h b/spm/ivy/ivy.h
deleted file mode 100644
index c5cac2e..0000000
--- a/spm/ivy/ivy.h
+++ /dev/null
@@ -1,29 +0,0 @@
-/*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#ifndef IVY_H
-#define IVY_H
-
-#include <stdint.h>
-
-/* Linker symbols used to figure out the memory layout of Ivy. */
-extern uintptr_t __TEXT_START__, __TEXT_END__;
-#define IVY_TEXT_START		((uintptr_t)&__TEXT_START__)
-#define IVY_TEXT_END		((uintptr_t)&__TEXT_END__)
-
-extern uintptr_t __RODATA_START__, __RODATA_END__;
-#define IVY_RODATA_START	((uintptr_t)&__RODATA_START__)
-#define IVY_RODATA_END		((uintptr_t)&__RODATA_END__)
-
-extern uintptr_t __DATA_START__, __DATA_END__;
-#define IVY_DATA_START		((uintptr_t)&__DATA_START__)
-#define IVY_DATA_END		((uintptr_t)&__DATA_END__)
-
-extern uintptr_t __BSS_START__, __BSS_END__;
-#define IVY_BSS_START		((uintptr_t)&__BSS_START__)
-#define IVY_BSS_END		((uintptr_t)&__BSS_END__)
-
-#endif /* __IVY_H__ */
diff --git a/spm/ivy/ivy.ld.S b/spm/ivy/ivy.ld.S
index 634db15..0e47c21 100644
--- a/spm/ivy/ivy.ld.S
+++ b/spm/ivy/ivy.ld.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,7 +10,7 @@
 
 OUTPUT_FORMAT(PLATFORM_LINKER_FORMAT)
 OUTPUT_ARCH(PLATFORM_LINKER_ARCH)
-ENTRY(ivy_entrypoint)
+ENTRY(shim_entrypoint)
 
 SECTIONS
 {
@@ -19,6 +19,48 @@
     ASSERT(. == ALIGN(PAGE_SIZE),
            "TEXT_START address is not aligned to PAGE_SIZE.")
 
+    /*----------------- START S-EL1 SHIM ----------------*/
+
+    .shim_text : {
+        __SHIM_TEXT_START__ = .;
+        *spm_shim_entrypoint.o(.text*)
+        *(.vectors)
+        . = NEXT(PAGE_SIZE);
+        __SHIM_TEXT_END__ = .;
+    }
+
+    .shim_rodata : {
+        . = ALIGN(PAGE_SIZE);
+        __SHIM_RODATA_START__ = .;
+
+        . = NEXT(PAGE_SIZE);
+        __SHIM_RODATA_END__ = .;
+    }
+
+    .shim_data : {
+        . = ALIGN(PAGE_SIZE);
+        __SHIM_DATA_START__ = .;
+
+        . = NEXT(PAGE_SIZE);
+        __SHIM_DATA_END__ = .;
+    }
+
+    .shim_bss (NOLOAD) : {
+        . = ALIGN(PAGE_SIZE);
+        __SHIM_BSS_START__ = .;
+
+        *(.bss.shim_stacks)
+        *(.bss.tf_base_xlat_table)
+        *(.bss.tf_mmap)
+        *xlat_tables_context.o(COMMON)
+	*xlat_tables_context.o(xlat_table)
+
+        . = NEXT(PAGE_SIZE);
+        __SHIM_BSS_END__ = .;
+    }
+
+    /*----------------- END S-EL1 SHIM ----------------*/
+
     .text : {
         __TEXT_START__ = .;
         *ivy_entrypoint.o(.text*)
@@ -32,6 +74,17 @@
         . = ALIGN(PAGE_SIZE);
         __RODATA_START__ = .;
         *(.rodata*)
+
+        /*
+         * Keep the .got section in the RO section as it is patched
+         * prior to enabling the MMU, so having it in RO is better for
+         * security. GOT is a table of addresses so ensure 8-byte alignment.
+         */
+        . = ALIGN(8);
+        __GOT_START__ = .;
+        *(.got)
+        __GOT_END__ = .;
+
         . = NEXT(PAGE_SIZE);
         __RODATA_END__ = .;
     }
@@ -44,6 +97,18 @@
         __DATA_END__ = .;
     }
 
+    /*
+     * .rela.dyn needs to come after .data for the read-elf utility
+     * to parse this section correctly. Ensure 8-byte alignment so
+     * that the fields of RELA data structure are aligned.
+     */
+    . = ALIGN(8);
+    __RELA_START__ = .;
+    .rela.dyn . : {
+    }
+    __RELA_END__ = .;
+
+
     .bss (NOLOAD) : {
         . = ALIGN(PAGE_SIZE);
         __BSS_START__ = .;
diff --git a/spm/ivy/ivy.mk b/spm/ivy/ivy.mk
index a500049..d184097 100644
--- a/spm/ivy/ivy.mk
+++ b/spm/ivy/ivy.mk
@@ -1,51 +1,67 @@
 #
-# Copyright (c) 2018-2020, Arm Limited. All rights reserved.
+# Copyright (c) 2018-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
 include branch_protection.mk
-include lib/sprt/sprt_client.mk
+include lib/xlat_tables_v2/xlat_tables.mk
 
-IVY_DTB		:= $(BUILD_PLAT)/ivy.dtb
+# Include ivy platform Makefile
+IVY_PLAT_PATH	:= $(shell find spm/ivy/app/plat -wholename '*/${PLAT}')
+ifneq (${IVY_PLAT_PATH},)
+	include ${IVY_PLAT_PATH}/platform.mk
+endif
+
+IVY_DTB		:= build/${PLAT}/debug/ivy.dtb
 
 IVY_INCLUDES :=					\
+	-Itftf/framework/include			\
 	-Iinclude					\
 	-Iinclude/common				\
 	-Iinclude/common/${ARCH}			\
 	-Iinclude/lib					\
 	-Iinclude/lib/${ARCH}				\
-	-Iinclude/lib/sprt				\
 	-Iinclude/lib/utils				\
 	-Iinclude/lib/xlat_tables			\
 	-Iinclude/runtime_services			\
 	-Iinclude/runtime_services/secure_el0_payloads	\
-	-Ispm/ivy					\
-	-Ispm/common					\
-	${SPRT_LIB_INCLUDES}
+	-Ispm/ivy/app				\
+	-Ispm/ivy/shim					\
+	-Ispm/common
 
 IVY_SOURCES	:=					\
-	$(addprefix spm/ivy/,			\
+	$(addprefix spm/ivy/app/,			\
 		aarch64/ivy_entrypoint.S		\
 		ivy_main.c				\
 	)						\
+	$(addprefix spm/ivy/shim/,			\
+		aarch64/spm_shim_entrypoint.S		\
+		aarch64/spm_shim_exceptions.S		\
+		shim_main.c				\
+	)						\
 	$(addprefix spm/common/,			\
 		aarch64/sp_arch_helpers.S		\
+		sp_debug.c				\
 		sp_helpers.c				\
+		spm_helpers.c				\
 	)						\
 
 # TODO: Remove dependency on TFTF files.
 IVY_SOURCES	+=					\
 	tftf/framework/debug.c				\
-	tftf/framework/${ARCH}/asm_debug.S
+	tftf/framework/${ARCH}/asm_debug.S		\
+	tftf/tests/runtime_services/secure_service/ffa_helpers.c
 
 IVY_SOURCES	+= 	drivers/arm/pl011/${ARCH}/pl011_console.S	\
-			drivers/console/console.c			\
 			lib/${ARCH}/cache_helpers.S			\
 			lib/${ARCH}/misc_helpers.S			\
+			lib/smc/${ARCH}/asm_smc.S			\
+			lib/smc/${ARCH}/smc.c				\
+			lib/smc/${ARCH}/hvc.c				\
 			lib/locks/${ARCH}/spinlock.S			\
 			lib/utils/mp_printf.c				\
-			${SPRT_LIB_SOURCES}
+			${XLAT_TABLES_LIB_SRCS}
 
 IVY_LINKERFILE	:=	spm/ivy/ivy.ld.S
 
@@ -64,12 +80,31 @@
 $(eval $(call add_define,IVY_DEFINES,PLAT_${PLAT}))
 
 $(IVY_DTB) : $(BUILD_PLAT)/ivy $(BUILD_PLAT)/ivy/ivy.elf
-$(IVY_DTB) : spm/ivy/ivy.dts
-	@echo "  DTBGEN  spm/ivy/ivy.dts"
+$(IVY_DTB) : $(IVY_DTS)
+	@echo "  DTBGEN  $@"
 	${Q}tools/generate_dtb/generate_dtb.sh \
-		ivy spm/ivy/ivy.dts $(BUILD_PLAT)
+		ivy ${IVY_DTS} $(BUILD_PLAT)
+	${Q}tools/generate_json/generate_json.sh \
+		ivy $(BUILD_PLAT)
 	@echo
 	@echo "Built $@ successfully"
 	@echo
 
 ivy: $(IVY_DTB)
+
+# FDTS_CP copies flattened device tree sources
+#   $(1) = output directory
+#   $(2) = flattened device tree source file to copy
+define FDTS_CP
+        $(eval FDTS := $(addprefix $(1)/,$(notdir $(2))))
+FDTS_LIST += $(FDTS)
+$(FDTS): $(2) $(IVY_DTB)
+	@echo "  CP      $$<"
+	${Q}cp $$< $$@
+endef
+
+ifdef FDTS_CP_LIST
+        $(eval files := $(filter %.dts,$(FDTS_CP_LIST)))
+        $(eval $(foreach file,$(files),$(call FDTS_CP,$(BUILD_PLAT),$(file))))
+ivy: $(FDTS_LIST)
+endif
diff --git a/spm/ivy/ivy_main.c b/spm/ivy/ivy_main.c
deleted file mode 100644
index 8542150..0000000
--- a/spm/ivy/ivy_main.c
+++ /dev/null
@@ -1,135 +0,0 @@
-/*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
- *
- * SPDX-License-Identifier: BSD-3-Clause
- */
-
-#include <assert.h>
-#include <debug.h>
-#include <drivers/console.h>
-#include <drivers/arm/pl011.h>
-#include <errno.h>
-#include <ivy_def.h>
-#include <plat_arm.h>
-#include <platform_def.h>
-#include <sp_helpers.h>
-#include <sprt_client.h>
-#include <sprt_svc.h>
-
-#include "ivy.h"
-#include "ivy_def.h"
-
-/* Host machine information injected by the build system in the ELF file. */
-extern const char build_message[];
-extern const char version_string[];
-
-static void ivy_print_memory_layout(void)
-{
-	NOTICE("Secure Partition memory layout:\n");
-
-	NOTICE("  Image regions\n");
-	NOTICE("    Text region            : %p - %p\n",
-		(void *)IVY_TEXT_START, (void *)IVY_TEXT_END);
-	NOTICE("    Read-only data region  : %p - %p\n",
-		(void *)IVY_RODATA_START, (void *)IVY_RODATA_END);
-	NOTICE("    Data region            : %p - %p\n",
-		(void *)IVY_DATA_START, (void *)IVY_DATA_END);
-	NOTICE("    BSS region             : %p - %p\n",
-		(void *)IVY_BSS_START, (void *)IVY_BSS_END);
-	NOTICE("    Total image memory     : %p - %p\n",
-		(void *)IVY_IMAGE_BASE,
-		(void *)(IVY_IMAGE_BASE + IVY_IMAGE_SIZE));
-	NOTICE("  SPM regions\n");
-	NOTICE("    SPM <-> SP buffer      : %p - %p\n",
-		(void *)IVY_SPM_BUF_BASE,
-		(void *)(IVY_SPM_BUF_BASE + IVY_SPM_BUF_SIZE));
-	NOTICE("    NS <-> SP buffer       : %p - %p\n",
-		(void *)IVY_NS_BUF_BASE,
-		(void *)(IVY_NS_BUF_BASE + IVY_NS_BUF_SIZE));
-}
-
-void ivy_message_handler(struct sprt_queue_entry_message *message)
-{
-	u_register_t ret0 = 0U, ret1 = 0U, ret2 = 0U, ret3 = 0U;
-
-	if (message->type == SPRT_MSG_TYPE_SERVICE_REQUEST) {
-		switch (message->args[1]) {
-
-		case IVY_PRINT_MAGIC:
-			INFO("IVY: Magic: 0x%x\n", IVY_MAGIC_NUMBER);
-			ret0 = SPRT_SUCCESS;
-			break;
-
-		case IVY_GET_MAGIC:
-			ret1 = IVY_MAGIC_NUMBER;
-			ret0 = SPRT_SUCCESS;
-			break;
-
-		case IVY_SLEEP_MS:
-			sp_sleep(message->args[2]);
-			ret0 = SPRT_SUCCESS;
-			break;
-
-		default:
-			NOTICE("IVY: Unhandled Service ID 0x%x\n",
-			       (unsigned int)message->args[1]);
-			ret0 = SPRT_NOT_SUPPORTED;
-			break;
-		}
-	} else {
-		NOTICE("Ivy: Unhandled Service type 0x%x\n",
-		       (unsigned int)message->type);
-		ret0 = SPRT_NOT_SUPPORTED;
-	}
-
-
-	sprt_message_end(message, ret0, ret1, ret2, ret3);
-}
-
-void __dead2 ivy_main(void)
-{
-	console_init(PL011_UART3_BASE,
-		     PL011_UART3_CLK_IN_HZ,
-		     PL011_BAUDRATE);
-
-	NOTICE("Booting test Secure Partition Ivy\n");
-	NOTICE("%s\n", build_message);
-	NOTICE("%s\n", version_string);
-	NOTICE("Running at S-EL0\n");
-
-	ivy_print_memory_layout();
-
-	/*
-	 * Handle secure service requests.
-	 */
-	sprt_initialize_queues((void *)IVY_SPM_BUF_BASE);
-
-	while (1) {
-		struct sprt_queue_entry_message message;
-
-		/*
-		 * Try to fetch a message from the blocking requests queue. If
-		 * it is empty, try to fetch from the non-blocking requests
-		 * queue. Repeat until both of them are empty.
-		 */
-		while (1) {
-			int err = sprt_get_next_message(&message,
-					SPRT_QUEUE_NUM_BLOCKING);
-			if (err == -ENOENT) {
-				err = sprt_get_next_message(&message,
-						SPRT_QUEUE_NUM_NON_BLOCKING);
-				if (err == -ENOENT) {
-					break;
-				} else {
-					assert(err == 0);
-					ivy_message_handler(&message);
-				}
-			} else {
-				assert(err == 0);
-				ivy_message_handler(&message);
-			}
-		}
-
-		sprt_wait_for_messages();
-	}
-}
diff --git a/spm/ivy/shim/aarch64/spm_shim_entrypoint.S b/spm/ivy/shim/aarch64/spm_shim_entrypoint.S
new file mode 100644
index 0000000..55d8dd8
--- /dev/null
+++ b/spm/ivy/shim/aarch64/spm_shim_entrypoint.S
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <asm_macros.S>
+#include <ivy_def.h>
+#include <platform_def.h>
+
+	.globl	shim_entrypoint
+
+.section .bss.shim_stacks
+	.balign CACHE_WRITEBACK_GRANULE
+	.fill	IVY_STACKS_SIZE
+shim_stacks_end:
+
+func shim_entrypoint
+
+	/* Setup the stack pointer. */
+	adr	x0, shim_stacks_end
+	mov	sp, x0
+
+	/* Setup vector base address */
+	adr	x0, spm_shim_exceptions_ptr
+	msr	vbar_el1, x0
+	isb
+
+	/*
+	 * Invalidate the data cache for the shim and whole partition.
+	 * This prevents re-use of stale data cache entries from prior
+	 * bootloader stages.
+	 */
+	adrp	x0, __SHIM_TEXT_START__
+	adrp	x1, __BSS_END__
+	sub	x1, x1, x0
+	bl	inv_dcache_range
+
+	/* Enable I-Cache */
+	mrs	x0, sctlr_el1
+	orr	x0, x0, #SCTLR_I_BIT
+	msr	sctlr_el1, x0
+	isb
+
+	/* Relocate symbols */
+shim_pie_fixup:
+	ldr	x0, =shim_pie_fixup
+	and	x0, x0, #~(0x1000 - 1)
+	mov	x1, #IVY_IMAGE_SIZE
+	add	x1, x1, x0
+	bl	fixup_gdt_reloc
+
+	/* Clear S-EL1 shim BSS */
+	adrp	x0, __SHIM_BSS_START__
+	adrp	x2, __SHIM_BSS_END__
+	sub	x2, x2, x0
+	mov	x1, xzr
+	bl	memset
+
+	/* Clear S-EL0 partition BSS */
+	adrp	x0, __BSS_START__
+	adrp	x2, __BSS_END__
+	sub	x2, x2, x0
+	mov	x1, xzr
+	bl	memset
+
+	/* And jump to the C entrypoint. */
+	bl	shim_main
+
+	/* Exception return to S-EL0 Ivy application code */
+	adrp	x0, ivy_entrypoint
+	msr	elr_el1, x0
+
+	/* AArch64 EL0t */
+	mov	x0, #((DAIF_FIQ_BIT | DAIF_IRQ_BIT) << SPSR_DAIF_SHIFT)
+	msr	spsr_el1, x0
+
+	/* TODO: clear GP/SIMD registers */
+	/* TODO: tune EL0 system registers */
+
+	eret
+
+endfunc shim_entrypoint
diff --git a/spm/ivy/shim/aarch64/spm_shim_exceptions.S b/spm/ivy/shim/aarch64/spm_shim_exceptions.S
new file mode 100644
index 0000000..07527e6
--- /dev/null
+++ b/spm/ivy/shim/aarch64/spm_shim_exceptions.S
@@ -0,0 +1,96 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+
+/* -----------------------------------------------------------------------------
+ * Very simple stackless exception handlers used by the spm shim layer.
+ * -----------------------------------------------------------------------------
+ */
+	.globl	spm_shim_exceptions_ptr
+
+vector_base spm_shim_exceptions_ptr
+
+	/* -----------------------------------------------------
+	 * Current EL with SP0 : 0x0 - 0x200
+	 * -----------------------------------------------------
+	 */
+vector_entry_spin sync_exception_sp_el0
+
+vector_entry_spin irq_sp_el0
+
+vector_entry_spin fiq_ep_el0
+
+vector_entry_spin serror_ep_el0
+
+	/* -----------------------------------------------------
+	 * Current EL with SPx: 0x200 - 0x400
+	 * -----------------------------------------------------
+	 */
+vector_entry_spin sync_exception_sp_elx
+
+vector_entry_spin irq_sp_elx
+
+vector_entry_spin fiq_sp_elx
+
+vector_entry_spin serror_sp_elx
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch64 : 0x400 - 0x600. No exceptions
+	 * are handled since secure_partition does not implement
+	 * a lower EL
+	 * -----------------------------------------------------
+	 */
+vector_entry sync_exception_aarch64
+	msr	tpidr_el1, x30
+	mrs	x30, esr_el1
+	ubfx	x30, x30, #ESR_EC_SHIFT, #ESR_EC_LENGTH
+
+	cmp	x30, #EC_AARCH64_SVC
+	b.eq 	do_smc
+
+	cmp	x30, #EC_AARCH32_SVC
+	b.eq	do_smc
+
+	cmp	x30, #EC_AARCH64_SYS
+	b.eq	handle_sys_trap
+
+	/* Fail in all the other cases */
+	b	panic
+
+	/* ---------------------------------------------
+	 * Tell SPM that we are done initialising
+	 * ---------------------------------------------
+	 */
+do_smc:
+	mrs	x30, tpidr_el1
+	smc	#0
+	eret
+
+	/* AArch64 system instructions trap are handled as a panic for now */
+handle_sys_trap:
+panic:
+	b	panic
+end_vector_entry sync_exception_aarch64
+
+vector_entry_spin irq_aarch64
+
+vector_entry_spin fiq_aarch64
+
+vector_entry_spin serror_aarch64
+
+	/* -----------------------------------------------------
+	 * Lower EL using AArch32 : 0x600 - 0x800
+	 * -----------------------------------------------------
+	 */
+vector_entry_spin sync_exception_aarch32
+
+vector_entry_spin irq_aarch32
+
+vector_entry_spin fiq_aarch32
+
+vector_entry_spin serror_aarch32
diff --git a/spm/ivy/shim/shim_main.c b/spm/ivy/shim/shim_main.c
new file mode 100644
index 0000000..97df1cc
--- /dev/null
+++ b/spm/ivy/shim/shim_main.c
@@ -0,0 +1,105 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <debug.h>
+#include <drivers/arm/pl011.h>
+#include <drivers/console.h>
+#include <errno.h>
+#include <ffa_helpers.h>
+#include <lib/aarch64/arch_helpers.h>
+#include <lib/xlat_tables/xlat_mmu_helpers.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat_arm.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+#include <sp_debug.h>
+#include <sp_helpers.h>
+#include <std_svc.h>
+
+#include "ivy.h"
+#include "ivy_def.h"
+
+static void shim_print_memory_layout(void)
+{
+	NOTICE("Secure Partition memory layout:\n");
+
+	NOTICE("  Image regions\n");
+	NOTICE("    Text region            : %p - %p\n",
+		(void *)IVY_TEXT_START, (void *)IVY_TEXT_END);
+	NOTICE("    Read-only data region  : %p - %p\n",
+		(void *)IVY_RODATA_START, (void *)IVY_RODATA_END);
+	NOTICE("    Data region            : %p - %p\n",
+		(void *)IVY_DATA_START, (void *)IVY_DATA_END);
+	NOTICE("    BSS region             : %p - %p\n",
+		(void *)IVY_BSS_START, (void *)IVY_BSS_END);
+	NOTICE("    Total image memory     : %p - %p\n",
+		(void *)IVY_IMAGE_BASE,
+		(void *)(IVY_IMAGE_BASE + IVY_IMAGE_SIZE));
+	NOTICE("  SPM regions\n");
+	NOTICE("    SPM <-> SP buffer      : %p - %p\n",
+		(void *)IVY_SPM_BUF_BASE,
+		(void *)(IVY_SPM_BUF_BASE + IVY_SPM_BUF_SIZE));
+	NOTICE("    NS <-> SP buffer       : %p - %p\n",
+		(void *)IVY_NS_BUF_BASE,
+		(void *)(IVY_NS_BUF_BASE + IVY_NS_BUF_SIZE));
+}
+
+static void shim_plat_configure_mmu(void)
+{
+	mmap_add_region(SHIM_TEXT_START,
+			SHIM_TEXT_START,
+			SHIM_TEXT_END - SHIM_TEXT_START,
+			MT_CODE | MT_PRIVILEGED);
+	mmap_add_region(SHIM_RODATA_START,
+			SHIM_RODATA_START,
+			SHIM_RODATA_END - SHIM_RODATA_START,
+			MT_RO_DATA | MT_PRIVILEGED);
+	mmap_add_region(SHIM_DATA_START,
+			SHIM_DATA_START,
+			SHIM_DATA_END - SHIM_DATA_START,
+			MT_RW_DATA | MT_PRIVILEGED);
+	mmap_add_region(SHIM_BSS_START,
+			SHIM_BSS_START,
+			SHIM_BSS_END - SHIM_BSS_START,
+			MT_RW_DATA | MT_PRIVILEGED);
+	mmap_add_region(IVY_TEXT_START,
+			IVY_TEXT_START,
+			IVY_TEXT_END - IVY_TEXT_START,
+			MT_CODE | MT_USER);
+	mmap_add_region(IVY_RODATA_START,
+			IVY_RODATA_START,
+			IVY_RODATA_END - IVY_RODATA_START,
+			MT_RO_DATA | MT_USER);
+	mmap_add_region(IVY_DATA_START,
+			IVY_DATA_START,
+			IVY_DATA_END - IVY_DATA_START,
+			MT_RW_DATA | MT_USER);
+	mmap_add_region(IVY_BSS_START,
+			IVY_BSS_START,
+			IVY_BSS_END - IVY_BSS_START,
+			MT_RW_DATA | MT_USER);
+
+	init_xlat_tables();
+}
+
+int shim_main(void)
+{
+	assert(IS_IN_EL1() != 0);
+
+	/* Initialise console */
+	set_putc_impl(HVC_CALL_AS_STDOUT);
+
+	NOTICE("Booting S-EL1 Shim\n");
+
+	/* Configure and enable Stage-1 MMU, enable D-Cache */
+	shim_plat_configure_mmu();
+	enable_mmu_el1(0);
+
+	shim_print_memory_layout();
+
+	return 0;
+}
diff --git a/tftf/framework/aarch64/exceptions.S b/tftf/framework/aarch64/exceptions.S
index 677b30f..218cca3 100644
--- a/tftf/framework/aarch64/exceptions.S
+++ b/tftf/framework/aarch64/exceptions.S
@@ -23,38 +23,38 @@
 	/*
 	 * Current EL with SP0 : 0x0 - 0x200.
 	 */
-unhandled_exception SynchronousExceptionSP0
-unhandled_exception IrqSP0
-unhandled_exception FiqSP0
-unhandled_exception SErrorSP0
+unhandled_exception sync_exception_sp_el0
+unhandled_exception irq_sp_el0
+unhandled_exception fiq_sp_el0
+unhandled_exception serror_sp_el0
 
 	/*
 	 * Current EL with SPx : 0x200 - 0x400.
 	 */
-unhandled_exception SynchronousExceptionSPx
+unhandled_exception sync_exception_sp_elx
 
-vector_entry IrqSPx
+vector_entry irq_sp_elx
 	b	irq_vector_entry
-end_vector_entry IrqSPx
+end_vector_entry irq_sp_elx
 
-unhandled_exception FiqSPx
-unhandled_exception SErrorSPx
+unhandled_exception fiq_sp_elx
+unhandled_exception serror_sp_elx
 
 	/*
 	 * Lower EL using AArch64 : 0x400 - 0x600.
 	 */
-unhandled_exception SynchronousExceptionA64
-unhandled_exception IrqA64
-unhandled_exception FiqA64
-unhandled_exception SErrorA64
+unhandled_exception sync_exception_aarch64
+unhandled_exception irq_aarch64
+unhandled_exception fiq_aarch64
+unhandled_exception serror_aarch64
 
 	/*
 	 * Lower EL using AArch32 : 0x600 - 0x800.
 	 */
-unhandled_exception SynchronousExceptionA32
-unhandled_exception IrqA32
-unhandled_exception FiqA32
-unhandled_exception SErrorA32
+unhandled_exception sync_exception_aarch32
+unhandled_exception irq_aarch32
+unhandled_exception fiq_aarch32
+unhandled_exception serror_aarch32
 
 .macro save_gp_regs
 	stp	x0, x1, [sp, #0x0]
diff --git a/tftf/tests/extensions/sve/test_sve.c b/tftf/tests/extensions/sve/test_sve.c
index 235e2b8..eabc0de 100644
--- a/tftf/tests/extensions/sve/test_sve.c
+++ b/tftf/tests/extensions/sve/test_sve.c
@@ -8,6 +8,7 @@
 #include <arch_helpers.h>
 #include <debug.h>
 #include <stdlib.h>
+#include <test_helpers.h>
 #include <tftf_lib.h>
 
 #include "./test_sve.h"
@@ -32,11 +33,7 @@
  */
 test_result_t test_sve_support(void)
 {
-	/* Check if SVE is implemented and usable */
-	if (is_armv8_2_sve_present() == false) {
-		tftf_testcase_printf("SVE support absent\n");
-		return TEST_RESULT_SKIPPED;
-	}
+	SKIP_TEST_IF_SVE_NOT_SUPPORTED();
 
 	for (int i = 0; i < SVE_ARRAYSIZE; i++) {
 		/* Generate a random number between 200 and 299 */
diff --git a/tftf/tests/misc_tests/inject_serror.S b/tftf/tests/misc_tests/inject_serror.S
index d42441d..fd51f85 100644
--- a/tftf/tests/misc_tests/inject_serror.S
+++ b/tftf/tests/misc_tests/inject_serror.S
@@ -27,6 +27,7 @@
 	/* Clear SError received flag if necessary */
 	cbz	x3, 1f
 	str	xzr, [x3, #0]
+	dsb	st
 1:
 	/* Choose Error record 0 on the PE */
 	msr	ERRSELR_EL1, x0
@@ -52,6 +53,7 @@
 
 2:
 	wfe
+	dsb	st
 	ldr	x0, [x3, #0]
 	cbz	x0, 2b
 
diff --git a/tftf/tests/runtime_services/secure_service/spm_common.c b/tftf/tests/runtime_services/secure_service/spm_common.c
index e2d3392..e2c7361 100644
--- a/tftf/tests/runtime_services/secure_service/spm_common.c
+++ b/tftf/tests/runtime_services/secure_service/spm_common.c
@@ -7,12 +7,17 @@
 #include <debug.h>
 #include <ffa_endpoints.h>
 #include <ffa_svc.h>
+#include <lib/extensions/sve.h>
 #include <spm_common.h>
 #include <xlat_tables_v2.h>
 
 #define __STR(x) #x
 #define STR(x) __STR(x)
-#define SIMD_TWO_VECTORS_BYTES_STR	(2 * SIMD_VECTOR_LEN_BYTES)
+
+#define fill_simd_helper(num1, num2) "ldp q"#num1", q"#num2",\
+	[%0], #"STR(2 * SIMD_VECTOR_LEN_BYTES)";"
+#define read_simd_helper(num1, num2) "stp q"#num1", q"#num2",\
+	[%0], #"STR(2 * SIMD_VECTOR_LEN_BYTES)";"
 
 /**
  * Helper to log errors after FF-A calls.
@@ -59,27 +64,26 @@
 
 	return false;
 }
-
 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) ";"
+		fill_simd_helper(0, 1)
+		fill_simd_helper(2, 3)
+		fill_simd_helper(4, 5)
+		fill_simd_helper(6, 7)
+		fill_simd_helper(8, 9)
+		fill_simd_helper(10, 11)
+		fill_simd_helper(12, 13)
+		fill_simd_helper(14, 15)
+		fill_simd_helper(16, 17)
+		fill_simd_helper(18, 19)
+		fill_simd_helper(20, 21)
+		fill_simd_helper(22, 23)
+		fill_simd_helper(24, 25)
+		fill_simd_helper(26, 27)
+		fill_simd_helper(28, 29)
+		fill_simd_helper(30, 31)
 		"sub %0, %0, #" STR(SIMD_NUM_VECTORS * SIMD_VECTOR_LEN_BYTES) ";"
 		: : "r" (v));
 #endif
@@ -89,29 +93,113 @@
 {
 #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) ";"
+		read_simd_helper(0, 1)
+		read_simd_helper(2, 3)
+		read_simd_helper(4, 5)
+		read_simd_helper(6, 7)
+		read_simd_helper(8, 9)
+		read_simd_helper(10, 11)
+		read_simd_helper(12, 13)
+		read_simd_helper(14, 15)
+		read_simd_helper(16, 17)
+		read_simd_helper(18, 19)
+		read_simd_helper(20, 21)
+		read_simd_helper(22, 23)
+		read_simd_helper(24, 25)
+		read_simd_helper(26, 27)
+		read_simd_helper(28, 29)
+		read_simd_helper(30, 31)
 		"sub %0, %0, #" STR(SIMD_NUM_VECTORS * SIMD_VECTOR_LEN_BYTES) ";"
 		: : "r" (v));
 #endif
 }
 
+void fill_sve_vector_regs(const sve_vector_t v[SVE_NUM_VECTORS])
+{
+#ifdef __aarch64__
+	__asm__ volatile(
+		".arch_extension sve\n"
+		fill_sve_helper(0)
+		fill_sve_helper(1)
+		fill_sve_helper(2)
+		fill_sve_helper(3)
+		fill_sve_helper(4)
+		fill_sve_helper(5)
+		fill_sve_helper(6)
+		fill_sve_helper(7)
+		fill_sve_helper(8)
+		fill_sve_helper(9)
+		fill_sve_helper(10)
+		fill_sve_helper(11)
+		fill_sve_helper(12)
+		fill_sve_helper(13)
+		fill_sve_helper(14)
+		fill_sve_helper(15)
+		fill_sve_helper(16)
+		fill_sve_helper(17)
+		fill_sve_helper(18)
+		fill_sve_helper(19)
+		fill_sve_helper(20)
+		fill_sve_helper(21)
+		fill_sve_helper(22)
+		fill_sve_helper(23)
+		fill_sve_helper(24)
+		fill_sve_helper(25)
+		fill_sve_helper(26)
+		fill_sve_helper(27)
+		fill_sve_helper(28)
+		fill_sve_helper(29)
+		fill_sve_helper(30)
+		fill_sve_helper(31)
+		".arch_extension nosve\n"
+		: : "r" (v));
+#endif
+}
+
+void read_sve_vector_regs(sve_vector_t v[SVE_NUM_VECTORS])
+{
+#ifdef __aarch64__
+	memset(v, 0, sizeof(sve_vector_t) * SVE_NUM_VECTORS);
+	__asm__ volatile(
+		".arch_extension sve\n"
+		read_sve_helper(0)
+		read_sve_helper(1)
+		read_sve_helper(2)
+		read_sve_helper(3)
+		read_sve_helper(4)
+		read_sve_helper(5)
+		read_sve_helper(6)
+		read_sve_helper(7)
+		read_sve_helper(8)
+		read_sve_helper(9)
+		read_sve_helper(10)
+		read_sve_helper(11)
+		read_sve_helper(12)
+		read_sve_helper(13)
+		read_sve_helper(14)
+		read_sve_helper(15)
+		read_sve_helper(16)
+		read_sve_helper(17)
+		read_sve_helper(18)
+		read_sve_helper(19)
+		read_sve_helper(20)
+		read_sve_helper(21)
+		read_sve_helper(22)
+		read_sve_helper(23)
+		read_sve_helper(24)
+		read_sve_helper(25)
+		read_sve_helper(26)
+		read_sve_helper(27)
+		read_sve_helper(28)
+		read_sve_helper(29)
+		read_sve_helper(30)
+		read_sve_helper(31)
+		".arch_extension nosve\n"
+		: : "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
index f57fa24..655f9d9 100644
--- a/tftf/tests/runtime_services/secure_service/test_spm_cpu_features.c
+++ b/tftf/tests/runtime_services/secure_service/test_spm_cpu_features.c
@@ -14,15 +14,11 @@
 
 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])
+static test_result_t fp_vector_compare(uint8_t *a, uint8_t *b,
+	size_t vector_size, uint8_t vectors_num)
 {
-	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;
-		}
+	if (memcmp(a, b, vector_size * vectors_num) != 0) {
+		return TEST_RESULT_FAIL;
 	}
 	return TEST_RESULT_SUCCESS;
 }
@@ -35,8 +31,6 @@
  */
 test_result_t test_simd_vectors_preserved(void)
 {
-	SKIP_TEST_IF_AARCH32();
-
 	/**********************************************************************
 	 * Verify that FFA is there and that it has the correct version.
 	 **********************************************************************/
@@ -50,7 +44,6 @@
 	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);
@@ -65,5 +58,50 @@
 
 	read_simd_vector_regs(simd_vectors_receive);
 
-	return simd_vector_compare(simd_vectors_send, simd_vectors_receive);
+	return fp_vector_compare((uint8_t *)simd_vectors_send,
+				 (uint8_t *)simd_vectors_receive,
+				 sizeof(simd_vector_t), SIMD_NUM_VECTORS);
+}
+
+/*
+ * Tests that SVE vectors are preserved during the context switches between
+ * normal world and the secure world.
+ * Fills the SVE 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_sve_vectors_preserved(void)
+{
+	SKIP_TEST_IF_SVE_NOT_SUPPORTED();
+
+	/**********************************************************************
+	 * Verify that FFA is there and that it has the correct version.
+	 **********************************************************************/
+	CHECK_SPMC_TESTING_SETUP(1, 0, expected_sp_uuids);
+
+	sve_vector_t sve_vectors_send[SVE_NUM_VECTORS],
+		     sve_vectors_receive[SVE_NUM_VECTORS];
+
+	/* 0x11 is just a dummy value to be distinguished from the value in the
+	 * secure world. */
+	for (unsigned int num = 0U; num < SVE_NUM_VECTORS; num++) {
+		memset(sve_vectors_send[num], 0x11 * num, sizeof(sve_vector_t));
+	}
+
+	fill_sve_vector_regs(sve_vectors_send);
+
+	smc_ret_values ret = cactus_req_simd_fill_send_cmd(SENDER, RECEIVER);
+
+	if (!is_ffa_direct_response(ret)) {
+		return TEST_RESULT_FAIL;
+	}
+
+	if (cactus_get_response(ret) == CACTUS_ERROR) {
+		return TEST_RESULT_FAIL;
+	}
+
+	read_sve_vector_regs(sve_vectors_receive);
+
+	return fp_vector_compare((uint8_t *)sve_vectors_send,
+				 (uint8_t *)sve_vectors_receive,
+				 sizeof(sve_vector_t), SVE_NUM_VECTORS);
 }
diff --git a/tftf/tests/tests-spm.xml b/tftf/tests/tests-spm.xml
index cafbc46..ea76778 100644
--- a/tftf/tests/tests-spm.xml
+++ b/tftf/tests/tests-spm.xml
@@ -70,6 +70,8 @@
              description="Validate context switch between NWd and SWd" >
      <testcase name="Check that SIMD registers context is preserved"
                function="test_simd_vectors_preserved" />
+     <testcase name="Check that SVE registers context is preserved"
+               function="test_sve_vectors_preserved" />
   </testsuite>
 
    <testsuite name="FF-A Interrupt"
diff --git a/tools/generate_json/generate_json.sh b/tools/generate_json/generate_json.sh
index f46cf15..3a3c04f 100755
--- a/tools/generate_json/generate_json.sh
+++ b/tools/generate_json/generate_json.sh
@@ -1,7 +1,7 @@
 #!/bin/bash
 
 #
-# Copyright (c) 2020, Arm Limited. All rights reserved.
+# Copyright (c) 2020-2021, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -28,7 +28,11 @@
 	\t\"pm\": \"$1-secondary.dts\",\n \
 	\t\"owner\": \"Plat\"\n\t},\n\n\t\"$1-tertiary\" : {\n \
 	\t\"image\": \"$1.bin\",\n \
-	\t\"pm\": \"$1-tertiary.dts\" \n \
+	\t\"pm\": \"$1-tertiary.dts\", \n \
+	\t\"owner\": \"Plat\"\n\t},\n\n\t\"ivy\" : {\n \
+	\t\"image\": \"ivy.bin\",\n \
+	\t\"pm\": \"ivy.dts\", \n \
+	\t\"owner\": \"Plat\"\n \
 	}\n}" \
 	> "$GENERATED_JSON"
 else