diff --git a/Makefile b/Makefile
index 03f9fc6..f3cb9be 100644
--- a/Makefile
+++ b/Makefile
@@ -412,40 +412,48 @@
 ################################################################################
 
 ifneq (${SPD},none)
-ifeq (${ARCH},aarch32)
+    ifeq (${ARCH},aarch32)
         $(error "Error: SPD is incompatible with AArch32.")
-endif
-ifdef EL3_PAYLOAD_BASE
+    endif
+
+    ifdef EL3_PAYLOAD_BASE
         $(warning "SPD and EL3_PAYLOAD_BASE are incompatible build options.")
         $(warning "The SPD and its BL32 companion will be present but ignored.")
-endif
-	ifeq (${SPD},spmd)
-		# SPMD is located in std_svc directory
-		SPD_DIR := std_svc
-	else
-		# All other SPDs in spd directory
-		SPD_DIR := spd
-	endif
-
-	# We expect to locate an spd.mk under the specified SPD directory
-	SPD_MAKE	:=	$(wildcard services/${SPD_DIR}/${SPD}/${SPD}.mk)
+    endif
 
+    ifeq (${SPD},spmd)
+        $(warning "SPMD is an experimental feature")
+        # SPMD is located in std_svc directory
+        SPD_DIR := std_svc
 
-        ifeq (${SPD_MAKE},)
-                $(error Error: No services/${SPD_DIR}/${SPD}/${SPD}.mk located)
+        ifeq ($(SPMD_SPM_AT_SEL2),1)
+            ifeq ($(CTX_INCLUDE_EL2_REGS),0)
+                $(error SPMD with SPM at S-EL2 requires CTX_INCLUDE_EL2_REGS option)
+            endif
         endif
-        $(info Including ${SPD_MAKE})
-        include ${SPD_MAKE}
+    else
+        # All other SPDs in spd directory
+        SPD_DIR := spd
+    endif
 
-        # If there's BL32 companion for the chosen SPD, we expect that the SPD's
-        # Makefile would set NEED_BL32 to "yes". In this case, the build system
-        # supports two mutually exclusive options:
-        # * BL32 is built from source: then BL32_SOURCES must contain the list
-        #   of source files to build BL32
-        # * BL32 is a prebuilt binary: then BL32 must point to the image file
-        #   that will be included in the FIP
-        # If both BL32_SOURCES and BL32 are defined, the binary takes precedence
-        # over the sources.
+    # We expect to locate an spd.mk under the specified SPD directory
+    SPD_MAKE	:=	$(wildcard services/${SPD_DIR}/${SPD}/${SPD}.mk)
+
+    ifeq (${SPD_MAKE},)
+        $(error Error: No services/${SPD_DIR}/${SPD}/${SPD}.mk located)
+    endif
+    $(info Including ${SPD_MAKE})
+    include ${SPD_MAKE}
+
+    # If there's BL32 companion for the chosen SPD, we expect that the SPD's
+    # Makefile would set NEED_BL32 to "yes". In this case, the build system
+    # supports two mutually exclusive options:
+    # * BL32 is built from source: then BL32_SOURCES must contain the list
+    #   of source files to build BL32
+    # * BL32 is a prebuilt binary: then BL32 must point to the image file
+    #   that will be included in the FIP
+    # If both BL32_SOURCES and BL32 are defined, the binary takes precedence
+    # over the sources.
 endif
 
 ################################################################################
@@ -761,6 +769,7 @@
 $(eval $(call assert_boolean,CTX_INCLUDE_FPREGS))
 $(eval $(call assert_boolean,CTX_INCLUDE_PAUTH_REGS))
 $(eval $(call assert_boolean,CTX_INCLUDE_MTE_REGS))
+$(eval $(call assert_boolean,CTX_INCLUDE_EL2_REGS))
 $(eval $(call assert_boolean,DEBUG))
 $(eval $(call assert_boolean,DYN_DISABLE_AUTH))
 $(eval $(call assert_boolean,EL3_EXCEPTION_HANDLING))
@@ -793,6 +802,7 @@
 $(eval $(call assert_boolean,SEPARATE_NOBITS_REGION))
 $(eval $(call assert_boolean,SPIN_ON_BL1_EXIT))
 $(eval $(call assert_boolean,SPM_MM))
+$(eval $(call assert_boolean,SPMD_SPM_AT_SEL2))
 $(eval $(call assert_boolean,TRUSTED_BOARD_BOOT))
 $(eval $(call assert_boolean,USE_COHERENT_MEM))
 $(eval $(call assert_boolean,USE_DEBUGFS))
@@ -832,6 +842,7 @@
 $(eval $(call add_define,CTX_INCLUDE_PAUTH_REGS))
 $(eval $(call add_define,EL3_EXCEPTION_HANDLING))
 $(eval $(call add_define,CTX_INCLUDE_MTE_REGS))
+$(eval $(call add_define,CTX_INCLUDE_EL2_REGS))
 $(eval $(call add_define,ENABLE_AMU))
 $(eval $(call add_define,ENABLE_ASSERTIONS))
 $(eval $(call add_define,ENABLE_BTI))
@@ -863,6 +874,7 @@
 $(eval $(call add_define,SPD_${SPD}))
 $(eval $(call add_define,SPIN_ON_BL1_EXIT))
 $(eval $(call add_define,SPM_MM))
+$(eval $(call add_define,SPMD_SPM_AT_SEL2))
 $(eval $(call add_define,TRUSTED_BOARD_BOOT))
 $(eval $(call add_define,USE_COHERENT_MEM))
 $(eval $(call add_define,USE_DEBUGFS))
diff --git a/bl32/tsp/tsp_main.c b/bl32/tsp/tsp_main.c
index e1d961c..9da2f9a 100644
--- a/bl32/tsp/tsp_main.c
+++ b/bl32/tsp/tsp_main.c
@@ -273,11 +273,11 @@
 	spin_lock(&console_lock);
 	INFO("TSP: cpu 0x%lx resumed. maximum off power level %lld\n",
 	     read_mpidr(), max_off_pwrlvl);
-	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu suspend requests\n",
+	INFO("TSP: cpu 0x%lx: %d smcs, %d erets %d cpu resume requests\n",
 		read_mpidr(),
 		tsp_stats[linear_id].smc_count,
 		tsp_stats[linear_id].eret_count,
-		tsp_stats[linear_id].cpu_suspend_count);
+		tsp_stats[linear_id].cpu_resume_count);
 	spin_unlock(&console_lock);
 #endif
 	/* Indicate to the SPD that we have completed this request */
diff --git a/drivers/arm/css/scp/css_pm_scmi.c b/drivers/arm/css/scp/css_pm_scmi.c
index 097d2eb..aeb7eda 100644
--- a/drivers/arm/css/scp/css_pm_scmi.c
+++ b/drivers/arm/css/scp/css_pm_scmi.c
@@ -224,8 +224,8 @@
 
 	SCMI_SET_PWR_STATE_MAX_PWR_LVL(scmi_pwr_state, lvl - 1);
 
-	core_pos = plat_core_pos_by_mpidr(mpidr);
-	assert(core_pos >= 0 && (core_pos < PLATFORM_CORE_COUNT));
+	core_pos = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+	assert(core_pos < PLATFORM_CORE_COUNT);
 
 	css_scp_core_pos_to_scmi_channel(core_pos, &domain_id,
 			&channel_id);
@@ -256,8 +256,8 @@
 		return PSCI_E_INVALID_PARAMS;
 	}
 
-	cpu_idx = plat_core_pos_by_mpidr(mpidr);
-	assert(cpu_idx > -1);
+	cpu_idx = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+	assert(cpu_idx < PLATFORM_CORE_COUNT);
 
 	css_scp_core_pos_to_scmi_channel(cpu_idx, &domain_id, &channel_id);
 	ret = scmi_pwr_state_get(scmi_handles[channel_id],
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 1faddbe..b0c2650 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -97,6 +97,32 @@
 #define ICC_SGI0R_EL1		S3_0_c12_c11_7
 
 /*******************************************************************************
+ * Definitions for EL2 system registers for save/restore routine
+ ******************************************************************************/
+
+#define CNTPOFF_EL2		S3_4_C14_C0_6
+#define HAFGRTR_EL2		S3_4_C3_C1_6
+#define HDFGRTR_EL2		S3_4_C3_C1_4
+#define HDFGWTR_EL2		S3_4_C3_C1_5
+#define HFGITR_EL2		S3_4_C1_C1_6
+#define HFGRTR_EL2		S3_4_C1_C1_4
+#define HFGWTR_EL2		S3_4_C1_C1_5
+#define ICH_HCR_EL2		S3_4_C12_C11_0
+#define ICH_VMCR_EL2		S3_4_C12_C11_7
+#define MPAMVPM0_EL2		S3_4_C10_C5_0
+#define MPAMVPM1_EL2		S3_4_C10_C5_1
+#define MPAMVPM2_EL2		S3_4_C10_C5_2
+#define MPAMVPM3_EL2		S3_4_C10_C5_3
+#define MPAMVPM4_EL2		S3_4_C10_C5_4
+#define MPAMVPM5_EL2		S3_4_C10_C5_5
+#define MPAMVPM6_EL2		S3_4_C10_C5_6
+#define MPAMVPM7_EL2		S3_4_C10_C5_7
+#define MPAMVPMV_EL2		S3_4_C10_C4_1
+#define TRFCR_EL2		S3_4_C1_C2_1
+#define PMSCR_EL2		S3_4_C9_C9_0
+#define TFSR_EL2		S3_4_C5_C6_0
+
+/*******************************************************************************
  * Generic timer memory mapped registers & offsets
  ******************************************************************************/
 #define CNTCR_OFF			U(0x000)
diff --git a/include/export/common/tbbr/tbbr_img_def_exp.h b/include/export/common/tbbr/tbbr_img_def_exp.h
index ff0d16c..3602554 100644
--- a/include/export/common/tbbr/tbbr_img_def_exp.h
+++ b/include/export/common/tbbr/tbbr_img_def_exp.h
@@ -86,6 +86,11 @@
 #define STM32_IMAGE_ID			U(29)
 
 /* Define size of the array */
+#if defined(SPD_spmd)
+#define MAX_SP_IDS			U(8)
+#define MAX_NUMBER_IDS			MAX_SP_IDS + U(30)
+#else
 #define MAX_NUMBER_IDS			U(30)
+#endif
 
 #endif /* ARM_TRUSTED_FIRMWARE_EXPORT_COMMON_TBBR_TBBR_IMG_DEF_EXP_H */
diff --git a/include/lib/el3_runtime/aarch64/context.h b/include/lib/el3_runtime/aarch64/context.h
index 4158c02..e061950 100644
--- a/include/lib/el3_runtime/aarch64/context.h
+++ b/include/lib/el3_runtime/aarch64/context.h
@@ -68,7 +68,7 @@
  * registers are only 32-bits wide but are stored as 64-bit values for
  * convenience
  ******************************************************************************/
-#define CTX_SYSREGS_OFFSET	(CTX_EL3STATE_OFFSET + CTX_EL3STATE_END)
+#define CTX_EL1_SYSREGS_OFFSET	(CTX_EL3STATE_OFFSET + CTX_EL3STATE_END)
 #define CTX_SPSR_EL1		U(0x0)
 #define CTX_ELR_EL1		U(0x8)
 #define CTX_SCTLR_EL1		U(0x10)
@@ -138,13 +138,118 @@
 /*
  * End of system registers.
  */
-#define CTX_SYSREGS_END		CTX_MTE_REGS_END
+#define CTX_EL1_SYSREGS_END		CTX_MTE_REGS_END
+
+/*
+ * EL2 register set
+ */
+
+#if CTX_INCLUDE_EL2_REGS
+/* For later discussion
+ * ICH_AP0R<n>_EL2
+ * ICH_AP1R<n>_EL2
+ * AMEVCNTVOFF0<n>_EL2
+ * AMEVCNTVOFF1<n>_EL2
+ * ICH_LR<n>_EL2
+ */
+#define CTX_EL2_SYSREGS_OFFSET	(CTX_EL1_SYSREGS_OFFSET + CTX_EL1_SYSREGS_END)
+
+#define CTX_ACTLR_EL2		U(0x0)
+#define CTX_AFSR0_EL2		U(0x8)
+#define CTX_AFSR1_EL2		U(0x10)
+#define CTX_AMAIR_EL2		U(0x18)
+#define CTX_CNTHCTL_EL2		U(0x20)
+#define CTX_CNTHP_CTL_EL2	U(0x28)
+#define CTX_CNTHP_CVAL_EL2	U(0x30)
+#define CTX_CNTHP_TVAL_EL2	U(0x38)
+#define CTX_CNTVOFF_EL2		U(0x40)
+#define CTX_CPTR_EL2		U(0x48)
+#define CTX_DBGVCR32_EL2	U(0x50)
+#define CTX_ELR_EL2		U(0x58)
+#define CTX_ESR_EL2		U(0x60)
+#define CTX_FAR_EL2		U(0x68)
+#define CTX_FPEXC32_EL2		U(0x70)
+#define CTX_HACR_EL2		U(0x78)
+#define CTX_HCR_EL2		U(0x80)
+#define CTX_HPFAR_EL2		U(0x88)
+#define CTX_HSTR_EL2		U(0x90)
+#define CTX_ICC_SRE_EL2		U(0x98)
+#define CTX_ICH_HCR_EL2		U(0xa0)
+#define CTX_ICH_VMCR_EL2	U(0xa8)
+#define CTX_MAIR_EL2		U(0xb0)
+#define CTX_MDCR_EL2		U(0xb8)
+#define CTX_PMSCR_EL2		U(0xc0)
+#define CTX_SCTLR_EL2		U(0xc8)
+#define CTX_SPSR_EL2		U(0xd0)
+#define CTX_SP_EL2		U(0xd8)
+#define CTX_TCR_EL2		U(0xe0)
+#define CTX_TRFCR_EL2		U(0xe8)
+#define CTX_TTBR0_EL2		U(0xf0)
+#define CTX_VBAR_EL2		U(0xf8)
+#define CTX_VMPIDR_EL2		U(0x100)
+#define CTX_VPIDR_EL2		U(0x108)
+#define CTX_VTCR_EL2		U(0x110)
+#define CTX_VTTBR_EL2		U(0x118)
+
+// Only if MTE registers in use
+#define CTX_TFSR_EL2		U(0x120)
+
+// Only if ENABLE_MPAM_FOR_LOWER_ELS==1
+#define CTX_MPAM2_EL2		U(0x128)
+#define CTX_MPAMHCR_EL2		U(0x130)
+#define CTX_MPAMVPM0_EL2	U(0x138)
+#define CTX_MPAMVPM1_EL2	U(0x140)
+#define CTX_MPAMVPM2_EL2	U(0x148)
+#define CTX_MPAMVPM3_EL2	U(0x150)
+#define CTX_MPAMVPM4_EL2	U(0x158)
+#define CTX_MPAMVPM5_EL2	U(0x160)
+#define CTX_MPAMVPM6_EL2	U(0x168)
+#define CTX_MPAMVPM7_EL2	U(0x170)
+#define CTX_MPAMVPMV_EL2	U(0x178)
+
+// Starting with Armv8.6
+#define CTX_HAFGRTR_EL2		U(0x180)
+#define CTX_HDFGRTR_EL2		U(0x188)
+#define CTX_HDFGWTR_EL2		U(0x190)
+#define CTX_HFGITR_EL2		U(0x198)
+#define CTX_HFGRTR_EL2		U(0x1a0)
+#define CTX_HFGWTR_EL2		U(0x1a8)
+#define CTX_CNTPOFF_EL2		U(0x1b0)
+
+// Starting with Armv8.4
+#define CTX_CNTHPS_CTL_EL2	U(0x1b8)
+#define CTX_CNTHPS_CVAL_EL2	U(0x1c0)
+#define CTX_CNTHPS_TVAL_EL2	U(0x1c8)
+#define CTX_CNTHVS_CTL_EL2	U(0x1d0)
+#define CTX_CNTHVS_CVAL_EL2	U(0x1d8)
+#define CTX_CNTHVS_TVAL_EL2	U(0x1e0)
+#define CTX_CNTHV_CTL_EL2	U(0x1e8)
+#define CTX_CNTHV_CVAL_EL2	U(0x1f0)
+#define CTX_CNTHV_TVAL_EL2	U(0x1f8)
+#define CTX_CONTEXTIDR_EL2	U(0x200)
+#define CTX_SDER32_EL2		U(0x208)
+#define CTX_TTBR1_EL2		U(0x210)
+#define CTX_VDISR_EL2		U(0x218)
+#define CTX_VNCR_EL2		U(0x220)
+#define CTX_VSESR_EL2		U(0x228)
+#define CTX_VSTCR_EL2		U(0x230)
+#define CTX_VSTTBR_EL2		U(0x238)
+
+// Starting with Armv8.5
+#define CTX_SCXTNUM_EL2		U(0x240)
+/* Align to the next 16 byte boundary */
+#define CTX_EL2_SYSREGS_END	U(0x250)
+#endif /* CTX_INCLUDE_EL2_REGS */
 
 /*******************************************************************************
  * Constants that allow assembler code to access members of and the 'fp_regs'
  * structure at their correct offsets.
  ******************************************************************************/
-#define CTX_FPREGS_OFFSET	(CTX_SYSREGS_OFFSET + CTX_SYSREGS_END)
+#if CTX_INCLUDE_EL2_REGS
+# define CTX_FPREGS_OFFSET	(CTX_EL2_SYSREGS_OFFSET + CTX_EL2_SYSREGS_END)
+#else
+# define CTX_FPREGS_OFFSET	(CTX_EL1_SYSREGS_OFFSET + CTX_EL1_SYSREGS_END)
+#endif
 #if CTX_INCLUDE_FPREGS
 #define CTX_FP_Q0		U(0x0)
 #define CTX_FP_Q1		U(0x10)
@@ -235,7 +340,10 @@
 
 /* Constants to determine the size of individual context structures */
 #define CTX_GPREG_ALL		(CTX_GPREGS_END >> DWORD_SHIFT)
-#define CTX_SYSREG_ALL		(CTX_SYSREGS_END >> DWORD_SHIFT)
+#define CTX_EL1_SYSREGS_ALL	(CTX_EL1_SYSREGS_END >> DWORD_SHIFT)
+#if CTX_INCLUDE_EL2_REGS
+# define CTX_EL2_SYSREGS_ALL	(CTX_EL2_SYSREGS_END >> DWORD_SHIFT)
+#endif
 #if CTX_INCLUDE_FPREGS
 # define CTX_FPREG_ALL		(CTX_FPREGS_END >> DWORD_SHIFT)
 #endif
@@ -256,10 +364,18 @@
 
 /*
  * AArch64 EL1 system register context structure for preserving the
- * architectural state during switches from one security state to
- * another in EL1.
+ * architectural state during world switches.
  */
-DEFINE_REG_STRUCT(el1_sys_regs, CTX_SYSREG_ALL);
+DEFINE_REG_STRUCT(el1_sysregs, CTX_EL1_SYSREGS_ALL);
+
+
+/*
+ * AArch64 EL2 system register context structure for preserving the
+ * architectural state during world switches.
+ */
+#if CTX_INCLUDE_EL2_REGS
+DEFINE_REG_STRUCT(el2_sysregs, CTX_EL2_SYSREGS_ALL);
+#endif
 
 /*
  * AArch64 floating point register context structure for preserving
@@ -304,7 +420,10 @@
 typedef struct cpu_context {
 	gp_regs_t gpregs_ctx;
 	el3_state_t el3state_ctx;
-	el1_sys_regs_t sysregs_ctx;
+	el1_sysregs_t el1_sysregs_ctx;
+#if CTX_INCLUDE_EL2_REGS
+	el2_sysregs_t el2_sysregs_ctx;
+#endif
 #if CTX_INCLUDE_FPREGS
 	fp_regs_t fpregs_ctx;
 #endif
@@ -319,7 +438,10 @@
 #if CTX_INCLUDE_FPREGS
 # define get_fpregs_ctx(h)	(&((cpu_context_t *) h)->fpregs_ctx)
 #endif
-#define get_sysregs_ctx(h)	(&((cpu_context_t *) h)->sysregs_ctx)
+#define get_el1_sysregs_ctx(h)	(&((cpu_context_t *) h)->el1_sysregs_ctx)
+#if CTX_INCLUDE_EL2_REGS
+# define get_el2_sysregs_ctx(h)	(&((cpu_context_t *) h)->el2_sysregs_ctx)
+#endif
 #define get_gpregs_ctx(h)	(&((cpu_context_t *) h)->gpregs_ctx)
 #define get_cve_2018_3639_ctx(h)	(&((cpu_context_t *) h)->cve_2018_3639_ctx)
 #if CTX_INCLUDE_PAUTH_REGS
@@ -333,8 +455,12 @@
  */
 CASSERT(CTX_GPREGS_OFFSET == __builtin_offsetof(cpu_context_t, gpregs_ctx), \
 	assert_core_context_gp_offset_mismatch);
-CASSERT(CTX_SYSREGS_OFFSET == __builtin_offsetof(cpu_context_t, sysregs_ctx), \
-	assert_core_context_sys_offset_mismatch);
+CASSERT(CTX_EL1_SYSREGS_OFFSET == __builtin_offsetof(cpu_context_t, el1_sysregs_ctx), \
+	assert_core_context_el1_sys_offset_mismatch);
+#if CTX_INCLUDE_EL2_REGS
+CASSERT(CTX_EL2_SYSREGS_OFFSET == __builtin_offsetof(cpu_context_t, el2_sysregs_ctx), \
+	assert_core_context_el2_sys_offset_mismatch);
+#endif
 #if CTX_INCLUDE_FPREGS
 CASSERT(CTX_FPREGS_OFFSET == __builtin_offsetof(cpu_context_t, fpregs_ctx), \
 	assert_core_context_fp_offset_mismatch);
@@ -387,8 +513,14 @@
 /*******************************************************************************
  * Function prototypes
  ******************************************************************************/
-void el1_sysregs_context_save(el1_sys_regs_t *regs);
-void el1_sysregs_context_restore(el1_sys_regs_t *regs);
+void el1_sysregs_context_save(el1_sysregs_t *regs);
+void el1_sysregs_context_restore(el1_sysregs_t *regs);
+
+#if CTX_INCLUDE_EL2_REGS
+void el2_sysregs_context_save(el2_sysregs_t *regs);
+void el2_sysregs_context_restore(el2_sysregs_t *regs);
+#endif
+
 #if CTX_INCLUDE_FPREGS
 void fpregs_context_save(fp_regs_t *regs);
 void fpregs_context_restore(fp_regs_t *regs);
diff --git a/include/lib/el3_runtime/context_mgmt.h b/include/lib/el3_runtime/context_mgmt.h
index 17955e3..b36cd3d 100644
--- a/include/lib/el3_runtime/context_mgmt.h
+++ b/include/lib/el3_runtime/context_mgmt.h
@@ -36,6 +36,11 @@
 void cm_prepare_el3_exit(uint32_t security_state);
 
 #ifdef __aarch64__
+#if CTX_INCLUDE_EL2_REGS
+void cm_el2_sysregs_context_save(uint32_t security_state);
+void cm_el2_sysregs_context_restore(uint32_t security_state);
+#endif
+
 void cm_el1_sysregs_context_save(uint32_t security_state);
 void cm_el1_sysregs_context_restore(uint32_t security_state);
 void cm_set_elr_el3(uint32_t security_state, uintptr_t entrypoint);
diff --git a/include/plat/arm/common/fconf_arm_sp_getter.h b/include/plat/arm/common/fconf_arm_sp_getter.h
new file mode 100644
index 0000000..38c30fb
--- /dev/null
+++ b/include/plat/arm/common/fconf_arm_sp_getter.h
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2020, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef FCONF_ARM_SP_GETTER_H
+#define FCONF_ARM_SP_GETTER_H
+
+#include <lib/fconf/fconf.h>
+#include <tools_share/uuid.h>
+
+/* arm_sp getter */
+#define arm__sp_getter(prop)	arm_sp.prop
+
+#define ARM_SP_MAX_SIZE		U(0x10000)
+
+struct arm_sp_t {
+	unsigned int		number_of_sp;
+	union uuid_helper_t	uuids[MAX_SP_IDS];
+	uintptr_t		load_addr[MAX_SP_IDS];
+};
+
+int fconf_populate_arm_sp(uintptr_t config);
+
+extern struct arm_sp_t arm_sp;
+
+extern bl_mem_params_node_t sp_mem_params_descs[MAX_SP_IDS];
+
+#endif /* FCONF_ARM_SP_GETTER_H */
diff --git a/include/services/spm_core_manifest.h b/include/services/spm_core_manifest.h
index 06ecc13..7874882 100644
--- a/include/services/spm_core_manifest.h
+++ b/include/services/spm_core_manifest.h
@@ -21,13 +21,6 @@
 	uint32_t minor_version;
 
 	/*
-	 * Run-Time Exception Level (mandatory):
-	 * - 1: SEL1
-	 * - 2: SEL2
-	 */
-	uint32_t runtime_el;
-
-	/*
 	 * Run-Time Execution state (optional):
 	 * - 0: AArch64 (default)
 	 * - 1: AArch32
diff --git a/include/services/spmd_svc.h b/include/services/spmd_svc.h
index 6e4caf2..a766dcf 100644
--- a/include/services/spmd_svc.h
+++ b/include/services/spmd_svc.h
@@ -11,7 +11,7 @@
 #include <services/spci_svc.h>
 #include <stdint.h>
 
-int32_t spmd_setup(void);
+int spmd_setup(void);
 uint64_t spmd_smc_handler(uint32_t smc_fid,
 			  uint64_t x1,
 			  uint64_t x2,
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index 9bd25ba..30ad7b7 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,11 @@
 #include <assert_macros.S>
 #include <context.h>
 
+#if CTX_INCLUDE_EL2_REGS
+	.global	el2_sysregs_context_save
+	.global	el2_sysregs_context_restore
+#endif
+
 	.global	el1_sysregs_context_save
 	.global	el1_sysregs_context_restore
 #if CTX_INCLUDE_FPREGS
@@ -19,6 +24,385 @@
 	.global	restore_gp_pmcr_pauth_regs
 	.global	el3_exit
 
+#if CTX_INCLUDE_EL2_REGS
+
+/* -----------------------------------------------------
+ * The following function strictly follows the AArch64
+ * PCS to use x9-x17 (temporary caller-saved registers)
+ * to save EL2 system register context. It assumes that
+ * 'x0' is pointing to a 'el2_sys_regs' structure where
+ * the register context will be saved.
+ *
+ * The following registers are not added.
+ * AMEVCNTVOFF0<n>_EL2
+ * AMEVCNTVOFF1<n>_EL2
+ * ICH_AP0R<n>_EL2
+ * ICH_AP1R<n>_EL2
+ * ICH_LR<n>_EL2
+ * -----------------------------------------------------
+ */
+
+func el2_sysregs_context_save
+	mrs	x9, actlr_el2
+	mrs	x10, afsr0_el2
+	stp	x9, x10, [x0, #CTX_ACTLR_EL2]
+
+	mrs	x11, afsr1_el2
+	mrs	x12, amair_el2
+	stp	x11, x12, [x0, #CTX_AFSR1_EL2]
+
+	mrs	x13, cnthctl_el2
+	mrs	x14, cnthp_ctl_el2
+	stp	x13, x14, [x0, #CTX_CNTHCTL_EL2]
+
+	mrs	x15, cnthp_cval_el2
+	mrs	x16, cnthp_tval_el2
+	stp	x15, x16, [x0, #CTX_CNTHP_CVAL_EL2]
+
+	mrs	x17, cntvoff_el2
+	mrs	x9, cptr_el2
+	stp	x17, x9, [x0, #CTX_CNTVOFF_EL2]
+
+	mrs	x10, dbgvcr32_el2
+	mrs	x11, elr_el2
+	stp	x10, x11, [x0, #CTX_DBGVCR32_EL2]
+
+	mrs	x14, esr_el2
+	mrs	x15, far_el2
+	stp	x14, x15, [x0, #CTX_ESR_EL2]
+
+	mrs	x16, fpexc32_el2
+	mrs	x17, hacr_el2
+	stp	x16, x17, [x0, #CTX_FPEXC32_EL2]
+
+	mrs	x9, hcr_el2
+	mrs	x10, hpfar_el2
+	stp	x9, x10, [x0, #CTX_HCR_EL2]
+
+	mrs	x11, hstr_el2
+	mrs	x12, ICC_SRE_EL2
+	stp	x11, x12, [x0, #CTX_HSTR_EL2]
+
+	mrs	x13, ICH_HCR_EL2
+	mrs	x14, ICH_VMCR_EL2
+	stp	x13, x14, [x0, #CTX_ICH_HCR_EL2]
+
+	mrs	x15, mair_el2
+	mrs	x16, mdcr_el2
+	stp	x15, x16, [x0, #CTX_MAIR_EL2]
+
+	mrs	x17, PMSCR_EL2
+	mrs	x9, sctlr_el2
+	stp	x17, x9, [x0, #CTX_PMSCR_EL2]
+
+	mrs	x10, spsr_el2
+	mrs	x11, sp_el2
+	stp	x10, x11, [x0, #CTX_SPSR_EL2]
+
+	mrs	x12, tcr_el2
+	mrs	x13, TRFCR_EL2
+	stp	x12, x13, [x0, #CTX_TCR_EL2]
+
+	mrs	x14, ttbr0_el2
+	mrs	x15, vbar_el2
+	stp	x14, x15, [x0, #CTX_TTBR0_EL2]
+
+	mrs	x16, vmpidr_el2
+	mrs	x17, vpidr_el2
+	stp	x16, x17, [x0, #CTX_VMPIDR_EL2]
+
+	mrs	x9, vtcr_el2
+	mrs	x10, vttbr_el2
+	stp	x9, x10, [x0, #CTX_VTCR_EL2]
+
+#if CTX_INCLUDE_MTE_REGS
+	mrs	x11, TFSR_EL2
+	str	x11, [x0, #CTX_TFSR_EL2]
+#endif
+
+#if ENABLE_MPAM_FOR_LOWER_ELS
+	mrs	x9, MPAM2_EL2
+	mrs	x10, MPAMHCR_EL2
+	stp	x9, x10, [x0, #CTX_MPAM2_EL2]
+
+	mrs	x11, MPAMVPM0_EL2
+	mrs	x12, MPAMVPM1_EL2
+	stp	x11, x12, [x0, #CTX_MPAMVPM0_EL2]
+
+	mrs	x13, MPAMVPM2_EL2
+	mrs	x14, MPAMVPM3_EL2
+	stp	x13, x14, [x0, #CTX_MPAMVPM2_EL2]
+
+	mrs	x15, MPAMVPM4_EL2
+	mrs	x16, MPAMVPM5_EL2
+	stp	x15, x16, [x0, #CTX_MPAMVPM4_EL2]
+
+	mrs	x17, MPAMVPM6_EL2
+	mrs	x9, MPAMVPM7_EL2
+	stp	x17, x9, [x0, #CTX_MPAMVPM6_EL2]
+
+	mrs	x10, MPAMVPMV_EL2
+	str	x10, [x0, #CTX_MPAMVPMV_EL2]
+#endif
+
+
+#if ARM_ARCH_AT_LEAST(8, 6)
+	mrs	x11, HAFGRTR_EL2
+	mrs	x12, HDFGRTR_EL2
+	stp	x11, x12, [x0, #CTX_HAFGRTR_EL2]
+
+	mrs	x13, HDFGWTR_EL2
+	mrs	x14, HFGITR_EL2
+	stp	x13, x14, [x0, #CTX_HDFGWTR_EL2]
+
+	mrs	x15, HFGRTR_EL2
+	mrs	x16, HFGWTR_EL2
+	stp	x15, x16, [x0, #CTX_HFGRTR_EL2]
+
+	mrs	x17, CNTPOFF_EL2
+	str	x17, [x0, #CTX_CNTPOFF_EL2]
+#endif
+
+#if ARM_ARCH_AT_LEAST(8, 4)
+	mrs	x9, cnthps_ctl_el2
+	mrs	x10, cnthps_cval_el2
+	stp	x9, x10, [x0, #CTX_CNTHPS_CTL_EL2]
+
+	mrs	x11, cnthps_tval_el2
+	mrs	x12, cnthvs_ctl_el2
+	stp	x11, x12, [x0, #CTX_CNTHPS_TVAL_EL2]
+
+	mrs	x13, cnthvs_cval_el2
+	mrs	x14, cnthvs_tval_el2
+	stp	x13, x14, [x0, #CTX_CNTHVS_CVAL_EL2]
+
+	mrs	x15, cnthv_ctl_el2
+	mrs	x16, cnthv_cval_el2
+	stp	x15, x16, [x0, #CTX_CNTHV_CTL_EL2]
+
+	mrs	x17, cnthv_tval_el2
+	mrs	x9, contextidr_el2
+	stp	x17, x9, [x0, #CTX_CNTHV_TVAL_EL2]
+
+	mrs	x10, sder32_el2
+	str	x10, [x0, #CTX_SDER32_EL2]
+
+	mrs	x11, ttbr1_el2
+	str	x11, [x0, #CTX_TTBR1_EL2]
+
+	mrs	x12, vdisr_el2
+	str	x12, [x0, #CTX_VDISR_EL2]
+
+	mrs	x13, vncr_el2
+	str	x13, [x0, #CTX_VNCR_EL2]
+
+	mrs	x14, vsesr_el2
+	str	x14, [x0, #CTX_VSESR_EL2]
+
+	mrs	x15, vstcr_el2
+	str	x15, [x0, #CTX_VSTCR_EL2]
+
+	mrs	x16, vsttbr_el2
+	str	x16, [x0, #CTX_VSTTBR_EL2]
+#endif
+
+#if ARM_ARCH_AT_LEAST(8, 5)
+	mrs	x17, scxtnum_el2
+	str	x17, [x0, #CTX_SCXTNUM_EL2]
+#endif
+
+	ret
+endfunc el2_sysregs_context_save
+
+/* -----------------------------------------------------
+ * The following function strictly follows the AArch64
+ * PCS to use x9-x17 (temporary caller-saved registers)
+ * to restore EL2 system register context.  It assumes
+ * that 'x0' is pointing to a 'el2_sys_regs' structure
+ * from where the register context will be restored
+
+ * The following registers are not restored
+ * AMEVCNTVOFF0<n>_EL2
+ * AMEVCNTVOFF1<n>_EL2
+ * ICH_AP0R<n>_EL2
+ * ICH_AP1R<n>_EL2
+ * ICH_LR<n>_EL2
+ * -----------------------------------------------------
+ */
+func el2_sysregs_context_restore
+
+	ldp	x9, x10, [x0, #CTX_ACTLR_EL2]
+	msr	actlr_el2, x9
+	msr	afsr0_el2, x10
+
+	ldp	x11, x12, [x0, #CTX_AFSR1_EL2]
+	msr	afsr1_el2, x11
+	msr	amair_el2, x12
+
+	ldp	x13, x14, [x0, #CTX_CNTHCTL_EL2]
+	msr	cnthctl_el2, x13
+	msr	cnthp_ctl_el2, x14
+
+	ldp	x15, x16, [x0, #CTX_CNTHP_CVAL_EL2]
+	msr	cnthp_cval_el2, x15
+	msr	cnthp_tval_el2, x16
+
+	ldp	x17, x9, [x0, #CTX_CNTVOFF_EL2]
+	msr	cntvoff_el2, x17
+	msr	cptr_el2, x9
+
+	ldp	x10, x11, [x0, #CTX_DBGVCR32_EL2]
+	msr	dbgvcr32_el2, x10
+	msr	elr_el2, x11
+
+	ldp	x14, x15, [x0, #CTX_ESR_EL2]
+	msr	esr_el2, x14
+	msr	far_el2, x15
+
+	ldp	x16, x17, [x0, #CTX_FPEXC32_EL2]
+	msr	fpexc32_el2, x16
+	msr	hacr_el2, x17
+
+	ldp	x9, x10, [x0, #CTX_HCR_EL2]
+	msr	hcr_el2, x9
+	msr	hpfar_el2, x10
+
+	ldp	x11, x12, [x0, #CTX_HSTR_EL2]
+	msr	hstr_el2, x11
+	msr	ICC_SRE_EL2, x12
+
+	ldp	x13, x14, [x0, #CTX_ICH_HCR_EL2]
+	msr	ICH_HCR_EL2, x13
+	msr	ICH_VMCR_EL2, x14
+
+	ldp	x15, x16, [x0, #CTX_MAIR_EL2]
+	msr	mair_el2, x15
+	msr	mdcr_el2, x16
+
+	ldp	x17, x9, [x0, #CTX_PMSCR_EL2]
+	msr	PMSCR_EL2, x17
+	msr	sctlr_el2, x9
+
+	ldp	x10, x11, [x0, #CTX_SPSR_EL2]
+	msr	spsr_el2, x10
+	msr	sp_el2, x11
+
+	ldp	x12, x13, [x0, #CTX_TCR_EL2]
+	msr	tcr_el2, x12
+	msr	TRFCR_EL2, x13
+
+	ldp	x14, x15, [x0, #CTX_TTBR0_EL2]
+	msr	ttbr0_el2, x14
+	msr	vbar_el2, x15
+
+	ldp	x16, x17, [x0, #CTX_VMPIDR_EL2]
+	msr	vmpidr_el2, x16
+	msr	vpidr_el2, x17
+
+	ldp	x9, x10, [x0, #CTX_VTCR_EL2]
+	msr	vtcr_el2, x9
+	msr	vttbr_el2, x10
+
+#if CTX_INCLUDE_MTE_REGS
+	ldr	x11, [x0, #CTX_TFSR_EL2]
+	msr	TFSR_EL2, x11
+#endif
+
+#if ENABLE_MPAM_FOR_LOWER_ELS
+	ldp	x9, x10, [x0, #CTX_MPAM2_EL2]
+	msr	MPAM2_EL2, x9
+	msr	MPAMHCR_EL2, x10
+
+	ldp	x11, x12, [x0, #CTX_MPAMVPM0_EL2]
+	msr	MPAMVPM0_EL2, x11
+	msr	MPAMVPM1_EL2, x12
+
+	ldp	x13, x14, [x0, #CTX_MPAMVPM2_EL2]
+	msr	MPAMVPM2_EL2, x13
+	msr	MPAMVPM3_EL2, x14
+
+	ldp	x15, x16, [x0, #CTX_MPAMVPM4_EL2]
+	msr	MPAMVPM4_EL2, x15
+	msr	MPAMVPM5_EL2, x16
+
+	ldp	x17, x9, [x0, #CTX_MPAMVPM6_EL2]
+	msr	MPAMVPM6_EL2, x17
+	msr	MPAMVPM7_EL2, x9
+
+	ldr	x10, [x0, #CTX_MPAMVPMV_EL2]
+	msr	MPAMVPMV_EL2, x10
+#endif
+
+#if ARM_ARCH_AT_LEAST(8, 6)
+	ldp	x11, x12, [x0, #CTX_HAFGRTR_EL2]
+	msr	HAFGRTR_EL2, x11
+	msr	HDFGRTR_EL2, x12
+
+	ldp	x13, x14, [x0, #CTX_HDFGWTR_EL2]
+	msr	HDFGWTR_EL2, x13
+	msr	HFGITR_EL2, x14
+
+	ldp	x15, x16, [x0, #CTX_HFGRTR_EL2]
+	msr	HFGRTR_EL2, x15
+	msr	HFGWTR_EL2, x16
+
+	ldr	x17, [x0, #CTX_CNTPOFF_EL2]
+	msr	CNTPOFF_EL2, x17
+#endif
+
+#if ARM_ARCH_AT_LEAST(8, 4)
+	ldp	x9, x10, [x0, #CTX_CNTHPS_CTL_EL2]
+	msr	cnthps_ctl_el2, x9
+	msr	cnthps_cval_el2, x10
+
+	ldp	x11, x12, [x0, #CTX_CNTHPS_TVAL_EL2]
+	msr	cnthps_tval_el2, x11
+	msr	cnthvs_ctl_el2, x12
+
+	ldp	x13, x14, [x0, #CTX_CNTHVS_CVAL_EL2]
+	msr	cnthvs_cval_el2, x13
+	msr	cnthvs_tval_el2, x14
+
+	ldp	x15, x16, [x0, #CTX_CNTHV_CTL_EL2]
+	msr	cnthv_ctl_el2, x15
+	msr	cnthv_cval_el2, x16
+
+	ldp	x17, x9, [x0, #CTX_CNTHV_TVAL_EL2]
+	msr	cnthv_tval_el2, x17
+	msr	contextidr_el2, x9
+
+	ldr	x10, [x0, #CTX_SDER32_EL2]
+	msr	sder32_el2, x10
+
+	ldr	x11, [x0, #CTX_TTBR1_EL2]
+	msr	ttbr1_el2, x11
+
+	ldr	x12, [x0, #CTX_VDISR_EL2]
+	msr	vdisr_el2, x12
+
+	ldr	x13, [x0, #CTX_VNCR_EL2]
+	msr	vncr_el2, x13
+
+	ldr	x14, [x0, #CTX_VSESR_EL2]
+	msr	vsesr_el2, x14
+
+	ldr	x15, [x0, #CTX_VSTCR_EL2]
+	msr	vstcr_el2, x15
+
+	ldr	x16, [x0, #CTX_VSTTBR_EL2]
+	msr	vsttbr_el2, x16
+#endif
+
+#if ARM_ARCH_AT_LEAST(8, 5)
+	ldr	x17, [x0, #CTX_SCXTNUM_EL2]
+	msr	scxtnum_el2, x17
+#endif
+
+	ret
+endfunc el2_sysregs_context_restore
+
+#endif /* CTX_INCLUDE_EL2_REGS */
+
 /* ------------------------------------------------------------------
  * The following function strictly follows the AArch64 PCS to use
  * x9-x17 (temporary caller-saved registers) to save EL1 system
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 546e39e..0314a85 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -234,7 +234,7 @@
 	 * and other EL2 registers are set up by cm_prepare_ns_entry() as they
 	 * are not part of the stored cpu_context.
 	 */
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_SCTLR_EL1, sctlr_elx);
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_SCTLR_EL1, sctlr_elx);
 
 	/*
 	 * Base the context ACTLR_EL1 on the current value, as it is
@@ -244,7 +244,7 @@
 	 * be zero.
 	 */
 	actlr_elx = read_actlr_el1();
-	write_ctx_reg((get_sysregs_ctx(ctx)), (CTX_ACTLR_EL1), (actlr_elx));
+	write_ctx_reg((get_el1_sysregs_ctx(ctx)), (CTX_ACTLR_EL1), (actlr_elx));
 
 	/*
 	 * Populate EL3 state so that we've the right context
@@ -336,7 +336,7 @@
 						 CTX_SCR_EL3);
 		if ((scr_el3 & SCR_HCE_BIT) != 0U) {
 			/* Use SCTLR_EL1.EE value to initialise sctlr_el2 */
-			sctlr_elx = read_ctx_reg(get_sysregs_ctx(ctx),
+			sctlr_elx = read_ctx_reg(get_el1_sysregs_ctx(ctx),
 							   CTX_SCTLR_EL1);
 			sctlr_elx &= SCTLR_EE_BIT;
 			sctlr_elx |= SCTLR_EL2_RES1;
@@ -530,6 +530,52 @@
 	cm_set_next_eret_context(security_state);
 }
 
+#if CTX_INCLUDE_EL2_REGS
+/*******************************************************************************
+ * Save EL2 sysreg context
+ ******************************************************************************/
+void cm_el2_sysregs_context_save(uint32_t security_state)
+{
+	u_register_t scr_el3 = read_scr();
+
+	/*
+	 * Always save the non-secure EL2 context, only save the
+	 * S-EL2 context if S-EL2 is enabled.
+	 */
+	if ((security_state == NON_SECURE) ||
+	    ((scr_el3 & SCR_EEL2_BIT) != 0U)) {
+		cpu_context_t *ctx;
+
+		ctx = cm_get_context(security_state);
+		assert(ctx != NULL);
+
+		el2_sysregs_context_save(get_el2_sysregs_ctx(ctx));
+	}
+}
+
+/*******************************************************************************
+ * Restore EL2 sysreg context
+ ******************************************************************************/
+void cm_el2_sysregs_context_restore(uint32_t security_state)
+{
+	u_register_t scr_el3 = read_scr();
+
+	/*
+	 * Always restore the non-secure EL2 context, only restore the
+	 * S-EL2 context if S-EL2 is enabled.
+	 */
+	if ((security_state == NON_SECURE) ||
+	    ((scr_el3 & SCR_EEL2_BIT) != 0U)) {
+		cpu_context_t *ctx;
+
+		ctx = cm_get_context(security_state);
+		assert(ctx != NULL);
+
+		el2_sysregs_context_restore(get_el2_sysregs_ctx(ctx));
+	}
+}
+#endif /* CTX_INCLUDE_EL2_REGS */
+
 /*******************************************************************************
  * The next four functions are used by runtime services to save and restore
  * EL1 context on the 'cpu_context' structure for the specified security
@@ -542,7 +588,7 @@
 	ctx = cm_get_context(security_state);
 	assert(ctx != NULL);
 
-	el1_sysregs_context_save(get_sysregs_ctx(ctx));
+	el1_sysregs_context_save(get_el1_sysregs_ctx(ctx));
 
 #if IMAGE_BL31
 	if (security_state == SECURE)
@@ -559,7 +605,7 @@
 	ctx = cm_get_context(security_state);
 	assert(ctx != NULL);
 
-	el1_sysregs_context_restore(get_sysregs_ctx(ctx));
+	el1_sysregs_context_restore(get_el1_sysregs_ctx(ctx));
 
 #if IMAGE_BL31
 	if (security_state == SECURE)
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 60958a1..9273469 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -188,6 +188,9 @@
 # Enable the Management Mode (MM)-based Secure Partition Manager implementation
 SPM_MM				:= 0
 
+# Use SPM at S-EL2 as a default config for SPMD
+SPMD_SPM_AT_SEL2		:= 1
+
 # Flag to introduce an infinite loop in BL1 just before it exits into the next
 # image. This is meant to help debugging the post-BL2 phase.
 SPIN_ON_BL1_EXIT		:= 0
@@ -262,3 +265,8 @@
 
 # Enable Link Time Optimization
 ENABLE_LTO			:= 0
+
+# Build flag to include EL2 registers in cpu context save and restore during
+# S-EL2 firmware entry/exit. This flag is to be used with SPD=spmd option.
+# Default is 0.
+CTX_INCLUDE_EL2_REGS		:= 0
diff --git a/plat/arm/board/fvp/fdts/fvp_fw_config.dts b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
index d0f6033..9a4a057 100644
--- a/plat/arm/board/fvp/fdts/fvp_fw_config.dts
+++ b/plat/arm/board/fvp/fdts/fvp_fw_config.dts
@@ -69,6 +69,12 @@
 		mbedtls_heap_size = <0x0>;
 	};
 
+	/*
+	 * Though TF-A is UUID RFC 4122 compliant meaning fields are stored in
+	 * network order (big endian), UUID's mentioned in this file are are
+	 * stored in machine order (little endian).
+	 * This will be fixed in future.
+	 */
 	arm-io_policies {
 		fip-handles {
 			compatible = "arm,io-fip-handle";
@@ -93,4 +99,17 @@
 			nt_fw_content_cert_uuid = <0xf3c1c48e 0x11e4635d 0xee87a9a7 0xa73fb240>;
 		};
 	};
+
+	secure-partitions {
+		compatible = "arm,sp";
+		cactus {
+			uuid = <0x1e67b5b4 0xe14f904a 0x13fb1fb8 0xcbdae1da>;
+			load-address = <0x7000000>;
+		};
+
+		ivy {
+			uuid = <0x092358d1 0xb94723f0 0x64447c82 0xc88f57f5>;
+			load-address = <0x7100000>;
+		};
+	};
 };
diff --git a/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts b/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
index e1c106f..c94a209 100644
--- a/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
+++ b/plat/arm/board/fvp/fdts/fvp_spmc_manifest.dts
@@ -11,7 +11,6 @@
 	attribute {
 		maj_ver = <0x0>;
 		min_ver = <0x9>;
-		runtime_el = <0x1>;
 		exec_state = <0x0>;
 		load_address = <0x0 0x6000000>;
 		entrypoint = <0x0 0x6000000>;
diff --git a/plat/arm/common/arm_bl2_setup.c b/plat/arm/common/arm_bl2_setup.c
index dd39208..136e65a 100644
--- a/plat/arm/common/arm_bl2_setup.c
+++ b/plat/arm/common/arm_bl2_setup.c
@@ -205,6 +205,13 @@
  ******************************************************************************/
 int arm_bl2_plat_handle_post_image_load(unsigned int image_id)
 {
+#if defined(SPD_spmd)
+	/* For Secure Partitions we don't need post processing */
+	if ((image_id >= (MAX_NUMBER_IDS - MAX_SP_IDS)) &&
+		(image_id < MAX_NUMBER_IDS)) {
+		return 0;
+	}
+#endif
 	return arm_bl2_handle_post_image_load(image_id);
 }
 
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index 17058d1..4fb85fb 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -182,6 +182,9 @@
 else
 ARM_IO_SOURCES		+=	plat/arm/common/arm_fconf_io_storage.c		\
 				plat/arm/common/fconf/arm_fconf_io.c
+ifeq (${SPD},spmd)
+ARM_IO_SOURCES		+=	plat/arm/common/fconf/arm_fconf_sp.c
+endif
 endif
 
 BL1_SOURCES		+=	drivers/io/io_fip.c				\
diff --git a/plat/arm/common/arm_image_load.c b/plat/arm/common/arm_image_load.c
index 2faaa76..593199d 100644
--- a/plat/arm/common/arm_image_load.c
+++ b/plat/arm/common/arm_image_load.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,9 @@
 #include <assert.h>
 #include <common/bl_common.h>
 #include <common/desc_image_load.h>
+#if defined(SPD_spmd)
+#include <plat/arm/common/fconf_arm_sp_getter.h>
+#endif
 #include <plat/arm/common/plat_arm.h>
 #include <plat/common/platform.h>
 
@@ -29,12 +32,62 @@
 		next_bl_params_cpy_ptr);
 }
 
+#if defined(SPD_spmd)
+/*******************************************************************************
+ * This function appends Secure Partitions to list of loadable images.
+ ******************************************************************************/
+void plat_add_sp_images_load_info(struct bl_load_info *load_info)
+{
+	bl_load_info_node_t *node_info = load_info->head;
+	unsigned int index = 0;
+
+	if (sp_mem_params_descs[index].image_id == 0) {
+		ERROR("No Secure Partition Image available\n");
+		return;
+	}
+
+	/* Traverse through the bl images list */
+	do {
+		node_info = node_info->next_load_info;
+	} while (node_info->next_load_info != NULL);
+
+	for (; index < MAX_SP_IDS; index++) {
+		/* Populate the image information */
+		node_info->image_id = sp_mem_params_descs[index].image_id;
+		node_info->image_info = &sp_mem_params_descs[index].image_info;
+
+		if ((index + 1U) == MAX_SP_IDS) {
+			INFO("Reached Max number of SPs\n");
+			return;
+		}
+
+		if (sp_mem_params_descs[index + 1U].image_id == 0) {
+			return;
+		}
+
+		node_info->next_load_info =
+			&sp_mem_params_descs[index + 1U].load_node_mem;
+		node_info = node_info->next_load_info;
+
+	}
+}
+#endif
+
 /*******************************************************************************
  * This function returns the list of loadable images.
  ******************************************************************************/
 struct bl_load_info *plat_get_bl_image_load_info(void)
 {
+#if defined(SPD_spmd)
+	bl_load_info_t *bl_load_info;
+
+	bl_load_info = get_bl_load_info_from_mem_params_desc();
+	plat_add_sp_images_load_info(bl_load_info);
+
+	return bl_load_info;
+#else
 	return get_bl_load_info_from_mem_params_desc();
+#endif
 }
 
 /*******************************************************************************
diff --git a/plat/arm/common/fconf/arm_fconf_sp.c b/plat/arm/common/fconf/arm_fconf_sp.c
new file mode 100644
index 0000000..bb88aff
--- /dev/null
+++ b/plat/arm/common/fconf/arm_fconf_sp.c
@@ -0,0 +1,107 @@
+/*
+ * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <common/fdt_wrappers.h>
+#include <drivers/io/io_storage.h>
+#include <lib/object_pool.h>
+#include <libfdt.h>
+#include <plat/arm/common/arm_fconf_getter.h>
+#include <plat/arm/common/arm_fconf_io_storage.h>
+#include <plat/arm/common/fconf_arm_sp_getter.h>
+#include <platform_def.h>
+#include <tools_share/firmware_image_package.h>
+
+#ifdef IMAGE_BL2
+
+bl_mem_params_node_t sp_mem_params_descs[MAX_SP_IDS];
+
+struct arm_sp_t arm_sp;
+
+int fconf_populate_arm_sp(uintptr_t config)
+{
+	int sp_node, node, err;
+	union uuid_helper_t uuid_helper;
+	unsigned int index = 0;
+	const unsigned int sp_start_index = MAX_NUMBER_IDS - MAX_SP_IDS;
+
+	/* As libfdt use void *, we can't avoid this cast */
+	const void *dtb = (void *)config;
+
+	/* Assert the node offset point to "arm,sp" compatible property */
+	const char *compatible_str = "arm,sp";
+
+	node = fdt_node_offset_by_compatible(dtb, -1, compatible_str);
+	if (node < 0) {
+		ERROR("FCONF: Can't find %s in dtb\n", compatible_str);
+		return node;
+	}
+
+	fdt_for_each_subnode(sp_node, dtb, node) {
+		err = fdtw_read_array(dtb, sp_node, "uuid", 4,
+				      &uuid_helper.word);
+		if (err < 0) {
+			ERROR("FCONF: cannot read SP uuid\n");
+			return -1;
+		}
+
+		arm_sp.uuids[index] = uuid_helper;
+
+		err = fdtw_read_cells(dtb, sp_node, "load-address", 1,
+			&arm_sp.load_addr[index]);
+		if (err < 0) {
+			ERROR("FCONF: cannot read SP load address\n");
+			return -1;
+		}
+
+		VERBOSE("FCONF: %s UUID %x-%x-%x-%x load_addr=%lx\n",
+			__func__,
+			uuid_helper.word[0],
+			uuid_helper.word[1],
+			uuid_helper.word[2],
+			uuid_helper.word[3],
+			arm_sp.load_addr[index]);
+
+		/* Add SP information in mem param descriptor */
+		sp_mem_params_descs[index].image_id = sp_start_index + index;
+		SET_PARAM_HEAD(&sp_mem_params_descs[index].image_info,
+					PARAM_IMAGE_BINARY, VERSION_2, 0);
+		sp_mem_params_descs[index].image_info.image_max_size =
+							ARM_SP_MAX_SIZE;
+		sp_mem_params_descs[index].next_handoff_image_id =
+							INVALID_IMAGE_ID;
+		sp_mem_params_descs[index].image_info.image_base =
+							arm_sp.load_addr[index];
+
+		/* Add SP information in IO policies structure */
+		policies[sp_start_index + index].image_spec =
+						(uintptr_t)&arm_sp.uuids[index];
+		policies[sp_start_index + index].dev_handle = &fip_dev_handle;
+		policies[sp_start_index + index].check = open_fip;
+
+		index++;
+
+		if (index >= MAX_SP_IDS) {
+			ERROR("FCONF: reached max number of SPs\n");
+			return -1;
+		}
+	}
+
+	if ((sp_node < 0) && (sp_node != -FDT_ERR_NOTFOUND)) {
+		ERROR("%d: fdt_for_each_subnode(): %d\n", __LINE__, node);
+		return sp_node;
+	}
+
+	arm_sp.number_of_sp = index;
+	return 0;
+}
+
+FCONF_REGISTER_POPULATOR(arm_sp, fconf_populate_arm_sp);
+
+#endif /* IMAGE_BL2 */
diff --git a/plat/common/plat_spmd_manifest.c b/plat/common/plat_spmd_manifest.c
index 4c78979..9c3dc71 100644
--- a/plat/common/plat_spmd_manifest.c
+++ b/plat/common/plat_spmd_manifest.c
@@ -37,12 +37,6 @@
 		return -ENOENT;
 	}
 
-	rc = fdtw_read_cells(fdt, node, "runtime_el", 1, &attr->runtime_el);
-	if (rc) {
-		ERROR("Missing SPM core runtime EL in manifest.\n");
-		return -ENOENT;
-	}
-
 	rc = fdtw_read_cells(fdt, node, "exec_state", 1, &attr->exec_state);
 	if (rc)
 		NOTICE("Execution state not specified in SPM core manifest.\n");
@@ -61,7 +55,6 @@
 
 	VERBOSE("SPM core manifest attribute section:\n");
 	VERBOSE("  version: %x.%x\n", attr->major_version, attr->minor_version);
-	VERBOSE("  runtime_el: 0x%x\n", attr->runtime_el);
 	VERBOSE("  binary_size: 0x%x\n", attr->binary_size);
 	VERBOSE("  load_address: 0x%llx\n", attr->load_address);
 	VERBOSE("  entrypoint: 0x%llx\n", attr->entrypoint);
diff --git a/plat/imx/common/aarch32/imx_uart_console.S b/plat/imx/common/aarch32/imx_uart_console.S
index 1c729b1..1a1229a 100644
--- a/plat/imx/common/aarch32/imx_uart_console.S
+++ b/plat/imx/common/aarch32/imx_uart_console.S
@@ -20,7 +20,7 @@
 	mov	r4, r3
 	cmp	r4, #0
 	beq	register_fail
-	str	r0, [r4, #CONSOLE_T_DRVDATA]
+	str	r0, [r4, #CONSOLE_T_BASE]
 
 	bl	console_imx_uart_core_init
 	cmp	r0, #0
@@ -35,16 +35,16 @@
 endfunc console_imx_uart_register
 
 func console_imx_uart_putc
-	ldr	r1, [r1, #CONSOLE_T_DRVDATA]
+	ldr	r1, [r1, #CONSOLE_T_BASE]
 	b console_imx_uart_core_putc
 endfunc console_imx_uart_putc
 
 func console_imx_uart_getc
-	ldr	r0, [r0, #CONSOLE_T_DRVDATA]
+	ldr	r0, [r0, #CONSOLE_T_BASE]
 	b console_imx_uart_core_getc
 endfunc console_imx_uart_getc
 
 func console_imx_uart_flush
-	ldr	r0, [r0, #CONSOLE_T_DRVDATA]
+	ldr	r0, [r0, #CONSOLE_T_BASE]
 	b console_imx_uart_core_flush
 endfunc console_imx_uart_flush
diff --git a/plat/imx/common/imx_uart_console.S b/plat/imx/common/imx_uart_console.S
index 3bdeea2..0cb4fb8 100644
--- a/plat/imx/common/imx_uart_console.S
+++ b/plat/imx/common/imx_uart_console.S
@@ -25,7 +25,7 @@
 	mov	x7, x30
 	mov	x6, x3
 	cbz	x6, register_fail
-	str	x0, [x6, #CONSOLE_T_DRVDATA]
+	str	x0, [x6, #CONSOLE_T_BASE]
 
 	bl	console_imx_uart_init
 	cbz	x0, register_fail
@@ -44,7 +44,7 @@
 endfunc console_imx_uart_init
 
 func console_imx_uart_putc
-	ldr	x1, [x1, #CONSOLE_T_DRVDATA]
+	ldr	x1, [x1, #CONSOLE_T_BASE]
 	cbz	x1, putc_error
 
 	/* Prepare '\r' to '\n' */
@@ -68,7 +68,7 @@
 endfunc console_imx_uart_putc
 
 func console_imx_uart_getc
-	ldr	x0, [x0, #CONSOLE_T_DRVDATA]
+	ldr	x0, [x0, #CONSOLE_T_BASE]
 	cbz	x0, getc_error
 1:
 	ldr	w1, [x0, #UTS]
diff --git a/plat/imx/common/lpuart_console.S b/plat/imx/common/lpuart_console.S
index d8dac2c..98b3588 100644
--- a/plat/imx/common/lpuart_console.S
+++ b/plat/imx/common/lpuart_console.S
@@ -20,7 +20,7 @@
 	mov	x7, x30
 	mov	x6, x3
 	cbz	x6, register_fail
-	str	x0, [x6, #CONSOLE_T_DRVDATA]
+	str	x0, [x6, #CONSOLE_T_BASE]
 
 	bl	console_lpuart_init
 	cbz	x0, register_fail
@@ -39,7 +39,7 @@
 endfunc console_lpuart_init
 
 func console_lpuart_putc
-	ldr	x1, [x1, #CONSOLE_T_DRVDATA]
+	ldr	x1, [x1, #CONSOLE_T_BASE]
 	cbz	x1, putc_error
 	/* Prepare '\r' to '\n' */
 	cmp	w0, #0xA
@@ -62,7 +62,7 @@
 endfunc console_lpuart_putc
 
 func console_lpuart_getc
-	ldr	x0, [x0, #CONSOLE_T_DRVDATA]
+	ldr	x0, [x0, #CONSOLE_T_BASE]
 	cbz	x0, getc_error
 	/* Check if the receive FIFO state */
 	ret
diff --git a/plat/nvidia/tegra/common/drivers/spe/shared_console.S b/plat/nvidia/tegra/common/drivers/spe/shared_console.S
index c783373..6df73ec 100644
--- a/plat/nvidia/tegra/common/drivers/spe/shared_console.S
+++ b/plat/nvidia/tegra/common/drivers/spe/shared_console.S
@@ -69,7 +69,7 @@
 	check_if_console_is_ready x0, x1, x2, register_fail
 
 	cbz	x3, register_fail
-	str	x0, [x3, #CONSOLE_T_DRVDATA]
+	str	x0, [x3, #CONSOLE_T_BASE]
 	mov	x0, x3
 	finish_console_register spe putc=1, getc=1, flush=1
 
@@ -132,7 +132,7 @@
 	 * --------------------------------------------------------
 	 */
 func console_spe_putc
-	ldr	x1, [x1, #CONSOLE_T_DRVDATA]
+	ldr	x1, [x1, #CONSOLE_T_BASE]
 	b	console_spe_core_putc
 endfunc console_spe_putc
 
@@ -183,6 +183,6 @@
 	 * ---------------------------------------------
 	 */
 func console_spe_flush
-	ldr	x0, [x0, #CONSOLE_T_DRVDATA]
+	ldr	x0, [x0, #CONSOLE_T_BASE]
 	b	console_spe_core_flush
 endfunc console_spe_flush
diff --git a/plat/nvidia/tegra/common/tegra_fiq_glue.c b/plat/nvidia/tegra/common/tegra_fiq_glue.c
index 60b5595..8e198ae 100644
--- a/plat/nvidia/tegra/common/tegra_fiq_glue.c
+++ b/plat/nvidia/tegra/common/tegra_fiq_glue.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -155,7 +155,7 @@
 {
 	cpu_context_t *ctx = cm_get_context(NON_SECURE);
 	gp_regs_t *gpregs_ctx = get_gpregs_ctx(ctx);
-	const el1_sys_regs_t *el1state_ctx = get_sysregs_ctx(ctx);
+	const el1_sysregs_t *el1state_ctx = get_el1_sysregs_ctx(ctx);
 	uint32_t cpu = plat_my_core_pos();
 	uint64_t val;
 
diff --git a/plat/socionext/uniphier/uniphier_io_storage.c b/plat/socionext/uniphier/uniphier_io_storage.c
index e89c835..77d1eaf 100644
--- a/plat/socionext/uniphier/uniphier_io_storage.c
+++ b/plat/socionext/uniphier/uniphier_io_storage.c
@@ -21,7 +21,7 @@
 #include "uniphier.h"
 
 #define UNIPHIER_ROM_REGION_BASE	0x00000000ULL
-#define UNIPHIER_ROM_REGION_SIZE	0x10000000ULL
+#define UNIPHIER_ROM_REGION_SIZE	0x04000000ULL
 
 #define UNIPHIER_OCM_REGION_SIZE	0x00040000ULL
 
diff --git a/services/spd/trusty/trusty.c b/services/spd/trusty/trusty.c
index 092ffa8..ba2f4a6 100644
--- a/services/spd/trusty/trusty.c
+++ b/services/spd/trusty/trusty.c
@@ -150,9 +150,9 @@
 	(void)memcpy(&ctx->fiq_gpregs, get_gpregs_ctx(handle), sizeof(ctx->fiq_gpregs));
 	ctx->fiq_pc = SMC_GET_EL3(handle, CTX_ELR_EL3);
 	ctx->fiq_cpsr = SMC_GET_EL3(handle, CTX_SPSR_EL3);
-	ctx->fiq_sp_el1 = read_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1);
+	ctx->fiq_sp_el1 = read_ctx_reg(get_el1_sysregs_ctx(handle), CTX_SP_EL1);
 
-	write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp);
+	write_ctx_reg(get_el1_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_handler_sp);
 	cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_handler_pc, (uint32_t)ctx->fiq_handler_cpsr);
 
 	SMC_RET0(handle);
@@ -211,7 +211,7 @@
 	 */
 	(void)memcpy(get_gpregs_ctx(handle), &ctx->fiq_gpregs, sizeof(ctx->fiq_gpregs));
 	ctx->fiq_handler_active = 0;
-	write_ctx_reg(get_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1);
+	write_ctx_reg(get_el1_sysregs_ctx(handle), CTX_SP_EL1, ctx->fiq_sp_el1);
 	cm_set_elr_spsr_el3(NON_SECURE, ctx->fiq_pc, (uint32_t)ctx->fiq_cpsr);
 
 	SMC_RET0(handle);
diff --git a/services/std_svc/spm_mm/spm_mm_setup.c b/services/std_svc/spm_mm/spm_mm_setup.c
index ccb2f90..468e5b3 100644
--- a/services/std_svc/spm_mm/spm_mm_setup.c
+++ b/services/std_svc/spm_mm/spm_mm_setup.c
@@ -116,17 +116,17 @@
 		      xlat_ctx->pa_max_address, xlat_ctx->va_max_address,
 		      EL1_EL0_REGIME);
 
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_MAIR_EL1,
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_MAIR_EL1,
 		      mmu_cfg_params[MMU_CFG_MAIR]);
 
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_TCR_EL1,
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_TCR_EL1,
 		      mmu_cfg_params[MMU_CFG_TCR]);
 
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_TTBR0_EL1,
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_TTBR0_EL1,
 		      mmu_cfg_params[MMU_CFG_TTBR0]);
 
 	/* Setup SCTLR_EL1 */
-	u_register_t sctlr_el1 = read_ctx_reg(get_sysregs_ctx(ctx), CTX_SCTLR_EL1);
+	u_register_t sctlr_el1 = read_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_SCTLR_EL1);
 
 	sctlr_el1 |=
 		/*SCTLR_EL1_RES1 |*/
@@ -160,7 +160,7 @@
 		SCTLR_UMA_BIT
 	);
 
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_SCTLR_EL1, sctlr_el1);
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_SCTLR_EL1, sctlr_el1);
 
 	/*
 	 * Setup other system registers
@@ -168,10 +168,10 @@
 	 */
 
 	/* Shim Exception Vector Base Address */
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_VBAR_EL1,
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_VBAR_EL1,
 			SPM_SHIM_EXCEPTIONS_PTR);
 
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_CNTKCTL_EL1,
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_CNTKCTL_EL1,
 		      EL0PTEN_BIT | EL0VTEN_BIT | EL0PCTEN_BIT | EL0VCTEN_BIT);
 
 	/*
@@ -181,7 +181,7 @@
 	 * TTA: Enable access to trace registers.
 	 * ZEN (v8.2): Trap SVE instructions and access to SVE registers.
 	 */
-	write_ctx_reg(get_sysregs_ctx(ctx), CTX_CPACR_EL1,
+	write_ctx_reg(get_el1_sysregs_ctx(ctx), CTX_CPACR_EL1,
 			CPACR_EL1_FPEN(CPACR_EL1_FP_TRAP_NONE));
 
 	/*
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 677f639..2cdf4f5 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -33,9 +33,25 @@
 /*******************************************************************************
  * SPM Core attribute information read from its manifest.
  ******************************************************************************/
-spmc_manifest_sect_attribute_t spmc_attrs;
+static spmc_manifest_sect_attribute_t spmc_attrs;
 
 /*******************************************************************************
+ * SPM Core entry point information. Discovered on the primary core and reused
+ * on secondary cores.
+ ******************************************************************************/
+static entry_point_info_t *spmc_ep_info;
+
+/*******************************************************************************
+ * Static function declaration.
+ ******************************************************************************/
+static int32_t	spmd_init(void);
+static int	spmd_spmc_init(void *rd_base, size_t rd_size);
+static uint64_t	spmd_spci_error_return(void *handle, int error_code);
+static uint64_t	spmd_smc_forward(uint32_t smc_fid, bool secure_origin,
+				 uint64_t x1, uint64_t x2, uint64_t x3,
+				 uint64_t x4, void *handle);
+
+/*******************************************************************************
  * This function takes an SP context pointer and performs a synchronous entry
  * into it.
  ******************************************************************************/
@@ -49,17 +65,19 @@
 
 	/* Restore the context assigned above */
 	cm_el1_sysregs_context_restore(SECURE);
+#if SPMD_SPM_AT_SEL2
+	cm_el2_sysregs_context_restore(SECURE);
+#endif
 	cm_set_next_eret_context(SECURE);
 
-	/* Invalidate TLBs at EL1. */
-	tlbivmalle1();
-	dsbish();
-
-	/* Enter Secure Partition */
+	/* Enter SPMC */
 	rc = spmd_spm_core_enter(&spmc_ctx->c_rt_ctx);
 
 	/* Save secure state */
 	cm_el1_sysregs_context_save(SECURE);
+#if SPMD_SPM_AT_SEL2
+	cm_el2_sysregs_context_save(SECURE);
+#endif
 
 	return rc;
 }
@@ -109,63 +127,21 @@
 }
 
 /*******************************************************************************
- * Initialize context of SPM core.
+ * Load SPMC manifest, init SPMC.
  ******************************************************************************/
-int32_t spmd_setup(void)
+static int spmd_spmc_init(void *rd_base, size_t rd_size)
 {
 	int rc;
-	void *rd_base;
-	size_t rd_size;
-	entry_point_info_t *spmc_ep_info;
-	uintptr_t rd_base_align;
-	uintptr_t rd_size_align;
 	uint32_t ep_attr;
-
-	spmc_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
-	if (!spmc_ep_info) {
-		WARN("No SPM core image provided by BL2 boot loader, Booting "
-		     "device without SP initialization. SMC`s destined for SPM "
-		     "core will return SMC_UNK\n");
-		return 1;
-	}
-
-	/* Under no circumstances will this parameter be 0 */
-	assert(spmc_ep_info->pc != 0U);
-
-	/*
-	 * Check if BL32 ep_info has a reference to 'tos_fw_config'. This will
-	 * be used as a manifest for the SPM core at the next lower EL/mode.
-	 */
-	if (spmc_ep_info->args.arg0 == 0U || spmc_ep_info->args.arg2 == 0U) {
-		ERROR("Invalid or absent SPM core manifest\n");
-		panic();
-	}
-
-	/* Obtain whereabouts of SPM core manifest */
-	rd_base = (void *) spmc_ep_info->args.arg0;
-	rd_size = spmc_ep_info->args.arg2;
-
-	rd_base_align = page_align((uintptr_t) rd_base, DOWN);
-	rd_size_align = page_align((uintptr_t) rd_size, UP);
-
-	/* Map the manifest in the SPMD translation regime first */
-	VERBOSE("SPM core manifest base : 0x%lx\n", rd_base_align);
-	VERBOSE("SPM core manifest size : 0x%lx\n", rd_size_align);
-	rc = mmap_add_dynamic_region((unsigned long long) rd_base_align,
-				     (uintptr_t) rd_base_align,
-				     rd_size_align,
-				     MT_RO_DATA);
-	if (rc < 0) {
-		ERROR("Error while mapping SPM core manifest (%d).\n", rc);
-		panic();
-	}
+	unsigned int linear_id = plat_my_core_pos();
+	spmd_spm_core_context_t *spm_ctx = &spm_core_context[linear_id];
 
 	/* Load the SPM core manifest */
 	rc = plat_spm_core_manifest_load(&spmc_attrs, rd_base, rd_size);
-	if (rc < 0) {
+	if (rc != 0) {
 		WARN("No or invalid SPM core manifest image provided by BL2 "
 		     "boot loader. ");
-		goto error;
+		return 1;
 	}
 
 	/*
@@ -177,22 +153,14 @@
 		WARN("Unsupported SPCI version (%x.%x) specified in SPM core "
 		     "manifest image provided by BL2 boot loader.\n",
 		     spmc_attrs.major_version, spmc_attrs.minor_version);
-		goto error;
+		return 1;
 	}
 
 	INFO("SPCI version (%x.%x).\n", spmc_attrs.major_version,
 	     spmc_attrs.minor_version);
 
-	/* Validate the SPM core runtime EL */
-	if ((spmc_attrs.runtime_el != MODE_EL1) &&
-	    (spmc_attrs.runtime_el != MODE_EL2)) {
-		WARN("Unsupported SPM core run time EL%x specified in "
-		     "manifest image provided by BL2 boot loader.\n",
-		     spmc_attrs.runtime_el);
-		goto error;
-	}
-
-	INFO("SPM core run time EL%x.\n", spmc_attrs.runtime_el);
+	INFO("SPM core run time EL%x.\n",
+	     SPMD_SPM_AT_SEL2 ? MODE_EL2 : MODE_EL1);
 
 	/* Validate the SPM core execution state */
 	if ((spmc_attrs.exec_state != MODE_RW_64) &&
@@ -200,42 +168,39 @@
 		WARN("Unsupported SPM core execution state %x specified in "
 		     "manifest image provided by BL2 boot loader.\n",
 		     spmc_attrs.exec_state);
-		goto error;
+		return 1;
 	}
 
 	INFO("SPM core execution state %x.\n", spmc_attrs.exec_state);
 
-	/* Ensure manifest has not requested S-EL2 in AArch32 state */
-	if ((spmc_attrs.exec_state == MODE_RW_32) &&
-	    (spmc_attrs.runtime_el == MODE_EL2)) {
-		WARN("Invalid combination of SPM core execution state (%x) "
-		     "and run time EL (%x).\n", spmc_attrs.exec_state,
-		     spmc_attrs.runtime_el);
-		goto error;
+#if SPMD_SPM_AT_SEL2
+	/* Ensure manifest has not requested AArch32 state in S-EL2 */
+	if (spmc_attrs.exec_state == MODE_RW_32) {
+		WARN("AArch32 state at S-EL2 is not supported.\n");
+		return 1;
 	}
 
 	/*
 	 * Check if S-EL2 is supported on this system if S-EL2
 	 * is required for SPM
 	 */
-	if (spmc_attrs.runtime_el == MODE_EL2) {
-		uint64_t sel2 = read_id_aa64pfr0_el1();
+	uint64_t sel2 = read_id_aa64pfr0_el1();
 
-		sel2 >>= ID_AA64PFR0_SEL2_SHIFT;
-		sel2 &= ID_AA64PFR0_SEL2_MASK;
+	sel2 >>= ID_AA64PFR0_SEL2_SHIFT;
+	sel2 &= ID_AA64PFR0_SEL2_MASK;
 
-		if (!sel2) {
-			WARN("SPM core run time EL: S-EL%x is not supported "
-			     "but specified in manifest image provided by "
-			     "BL2 boot loader.\n", spmc_attrs.runtime_el);
-			goto error;
-		}
+	if (!sel2) {
+		WARN("SPM core run time S-EL2 is not supported.");
+		return 1;
 	}
+#endif /* SPMD_SPM_AT_SEL2 */
 
 	/* Initialise an entrypoint to set up the CPU context */
 	ep_attr = SECURE | EP_ST_ENABLE;
-	if (read_sctlr_el3() & SCTLR_EE_BIT)
+	if (read_sctlr_el3() & SCTLR_EE_BIT) {
 		ep_attr |= EP_EE_BIG;
+	}
+
 	SET_PARAM_HEAD(spmc_ep_info, PARAM_EP, VERSION_1, ep_attr);
 	assert(spmc_ep_info->pc == BL32_BASE);
 
@@ -250,14 +215,22 @@
 						 DAIF_IRQ_BIT |
 						 DAIF_ABT_BIT);
 	} else {
-		spmc_ep_info->spsr = SPSR_64(spmc_attrs.runtime_el,
+
+#if SPMD_SPM_AT_SEL2
+		static const uint32_t runtime_el = MODE_EL2;
+#else
+		static const uint32_t runtime_el = MODE_EL1;
+#endif
+		spmc_ep_info->spsr = SPSR_64(runtime_el,
 					     MODE_SP_ELX,
 					     DISABLE_ALL_EXCEPTIONS);
 	}
 
 	/* Initialise SPM core context with this entry point information */
-	cm_setup_context(&(spm_core_context[plat_my_core_pos()].cpu_ctx),
-			 spmc_ep_info);
+	cm_setup_context(&spm_ctx->cpu_ctx, spmc_ep_info);
+
+	/* Reuse PSCI affinity states to mark this SPMC context as off */
+	spm_ctx->state = AFF_STATE_OFF;
 
 	INFO("SPM core setup done.\n");
 
@@ -265,23 +238,122 @@
 	bl31_register_bl32_init(&spmd_init);
 
 	return 0;
+}
 
-error:
-	WARN("Booting device without SPM initialization. "
-	     "SPCI SMCs destined for SPM core will return "
-	     "ENOTSUPPORTED\n");
+/*******************************************************************************
+ * Initialize context of SPM core.
+ ******************************************************************************/
+int spmd_setup(void)
+{
+	int rc;
+	void *rd_base;
+	size_t rd_size;
+	uintptr_t rd_base_align;
+	uintptr_t rd_size_align;
 
-	rc = mmap_remove_dynamic_region(rd_base_align, rd_size_align);
-	if (rc < 0) {
-		ERROR("Error while unmapping SPM core manifest (%d).\n",
-		      rc);
+	spmc_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (!spmc_ep_info) {
+		WARN("No SPM core image provided by BL2 boot loader, Booting "
+		     "device without SP initialization. SMC`s destined for SPM "
+		     "core will return SMC_UNK\n");
+		return 1;
+	}
+
+	/* Under no circumstances will this parameter be 0 */
+	assert(spmc_ep_info->pc != 0U);
+
+	/*
+	 * Check if BL32 ep_info has a reference to 'tos_fw_config'. This will
+	 * be used as a manifest for the SPM core at the next lower EL/mode.
+	 */
+	if (spmc_ep_info->args.arg0 == 0U || spmc_ep_info->args.arg2 == 0U) {
+		ERROR("Invalid or absent SPM core manifest\n");
 		panic();
 	}
 
-	return 1;
+	/* Obtain whereabouts of SPM core manifest */
+	rd_base = (void *) spmc_ep_info->args.arg0;
+	rd_size = spmc_ep_info->args.arg2;
+
+	rd_base_align = page_align((uintptr_t) rd_base, DOWN);
+	rd_size_align = page_align((uintptr_t) rd_size, UP);
+
+	/* Map the manifest in the SPMD translation regime first */
+	VERBOSE("SPM core manifest base : 0x%lx\n", rd_base_align);
+	VERBOSE("SPM core manifest size : 0x%lx\n", rd_size_align);
+	rc = mmap_add_dynamic_region((unsigned long long) rd_base_align,
+				     (uintptr_t) rd_base_align,
+				     rd_size_align,
+				     MT_RO_DATA);
+	if (rc != 0) {
+		ERROR("Error while mapping SPM core manifest (%d).\n", rc);
+		panic();
+	}
+
+	/* Load manifest, init SPMC */
+	rc = spmd_spmc_init(rd_base, rd_size);
+	if (rc != 0) {
+		int mmap_rc;
+
+		WARN("Booting device without SPM initialization. "
+		     "SPCI SMCs destined for SPM core will return "
+		     "ENOTSUPPORTED\n");
+
+		mmap_rc = mmap_remove_dynamic_region(rd_base_align,
+						     rd_size_align);
+		if (mmap_rc != 0) {
+			ERROR("Error while unmapping SPM core manifest (%d).\n",
+			      mmap_rc);
+			panic();
+		}
+
+		return rc;
+	}
+
+	return 0;
 }
 
 /*******************************************************************************
+ * Forward SMC to the other security state
+ ******************************************************************************/
+static uint64_t spmd_smc_forward(uint32_t smc_fid, bool secure_origin,
+				 uint64_t x1, uint64_t x2, uint64_t x3,
+				 uint64_t x4, void *handle)
+{
+	uint32_t secure_state_in = (secure_origin) ? SECURE : NON_SECURE;
+	uint32_t secure_state_out = (!secure_origin) ? SECURE : NON_SECURE;
+
+	/* Save incoming security state */
+	cm_el1_sysregs_context_save(secure_state_in);
+#if SPMD_SPM_AT_SEL2
+	cm_el2_sysregs_context_save(secure_state_in);
+#endif
+
+	/* Restore outgoing security state */
+	cm_el1_sysregs_context_restore(secure_state_out);
+#if SPMD_SPM_AT_SEL2
+	cm_el2_sysregs_context_restore(secure_state_out);
+#endif
+	cm_set_next_eret_context(secure_state_out);
+
+	SMC_RET8(cm_get_context(secure_state_out), smc_fid, x1, x2, x3, x4,
+			SMC_GET_GP(handle, CTX_GPREG_X5),
+			SMC_GET_GP(handle, CTX_GPREG_X6),
+			SMC_GET_GP(handle, CTX_GPREG_X7));
+}
+
+/*******************************************************************************
+ * Return SPCI_ERROR with specified error code
+ ******************************************************************************/
+static uint64_t spmd_spci_error_return(void *handle, int error_code)
+{
+	SMC_RET8(handle, SPCI_ERROR,
+		 SPCI_TARGET_INFO_MBZ, error_code,
+		 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ, SPCI_PARAM_MBZ,
+		 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ);
+}
+
+/*******************************************************************************
  * This function handles all SMCs in the range reserved for SPCI. Each call is
  * either forwarded to the other security state or handled by the SPM dispatcher
  ******************************************************************************/
@@ -289,19 +361,12 @@
 			  uint64_t x3, uint64_t x4, void *cookie, void *handle,
 			  uint64_t flags)
 {
-	uint32_t in_sstate;
-	uint32_t out_sstate;
-	int32_t ret;
 	spmd_spm_core_context_t *ctx = &spm_core_context[plat_my_core_pos()];
+	bool secure_origin;
+	int32_t ret;
 
 	/* Determine which security state this SMC originated from */
-	if (is_caller_secure(flags)) {
-		in_sstate = SECURE;
-		out_sstate = NON_SECURE;
-	} else {
-		in_sstate = NON_SECURE;
-		out_sstate = SECURE;
-	}
+	secure_origin = is_caller_secure(flags);
 
 	INFO("SPM: 0x%x, 0x%llx, 0x%llx, 0x%llx, 0x%llx, "
 	     "0x%llx, 0x%llx, 0x%llx\n",
@@ -316,20 +381,12 @@
 		 * this CPU. If so, then indicate that the SPM core initialised
 		 * unsuccessfully.
 		 */
-		if ((in_sstate == SECURE) && (ctx->state == SPMC_STATE_RESET))
+		if (secure_origin && (ctx->state == SPMC_STATE_RESET)) {
 			spmd_spm_core_sync_exit(x2);
-
-		/* Save incoming security state */
-		cm_el1_sysregs_context_save(in_sstate);
-
-		/* Restore outgoing security state */
-		cm_el1_sysregs_context_restore(out_sstate);
-		cm_set_next_eret_context(out_sstate);
+		}
 
-		SMC_RET8(cm_get_context(out_sstate), smc_fid, x1, x2, x3, x4,
-			 SMC_GET_GP(handle, CTX_GPREG_X5),
-			 SMC_GET_GP(handle, CTX_GPREG_X6),
-			 SMC_GET_GP(handle, CTX_GPREG_X7));
+		return spmd_smc_forward(smc_fid, secure_origin,
+					x1, x2, x3, x4, handle);
 		break; /* not reached */
 
 	case SPCI_VERSION:
@@ -353,29 +410,18 @@
 		 */
 
 		/*
-		 * Check if w1 holds a valid SPCI fid. This is an
+		 * Check if x1 holds a valid SPCI fid. This is an
 		 * optimization.
 		 */
-		if (!is_spci_fid(x1))
-			SMC_RET8(handle, SPCI_ERROR,
-				 SPCI_TARGET_INFO_MBZ, SPCI_ERROR_NOT_SUPPORTED,
-				 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ, SPCI_PARAM_MBZ,
-				 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ);
+		if (!is_spci_fid(x1)) {
+			return spmd_spci_error_return(handle,
+						      SPCI_ERROR_NOT_SUPPORTED);
+		}
 
 		/* Forward SMC from Normal world to the SPM core */
-		if (in_sstate == NON_SECURE) {
-			/* Save incoming security state */
-			cm_el1_sysregs_context_save(in_sstate);
-
-			/* Restore outgoing security state */
-			cm_el1_sysregs_context_restore(out_sstate);
-			cm_set_next_eret_context(out_sstate);
-
-			SMC_RET8(cm_get_context(out_sstate), smc_fid,
-				 x1, x2, x3, x4,
-				 SMC_GET_GP(handle, CTX_GPREG_X5),
-				 SMC_GET_GP(handle, CTX_GPREG_X6),
-				 SMC_GET_GP(handle, CTX_GPREG_X7));
+		if (!secure_origin) {
+			return spmd_smc_forward(smc_fid, secure_origin,
+						x1, x2, x3, x4, handle);
 		} else {
 			/*
 			 * Return success if call was from secure world i.e. all
@@ -387,6 +433,7 @@
 				 SMC_GET_GP(handle, CTX_GPREG_X6),
 				 SMC_GET_GP(handle, CTX_GPREG_X7));
 		}
+
 		break; /* not reached */
 
 	case SPCI_RX_RELEASE:
@@ -395,11 +442,9 @@
 	case SPCI_RXTX_UNMAP:
 	case SPCI_MSG_RUN:
 		/* This interface must be invoked only by the Normal world */
-		if (in_sstate == SECURE) {
-			SMC_RET8(handle, SPCI_ERROR,
-				 SPCI_TARGET_INFO_MBZ, SPCI_ERROR_NOT_SUPPORTED,
-				 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ, SPCI_PARAM_MBZ,
-				 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ);
+		if (secure_origin) {
+			return spmd_spci_error_return(handle,
+						      SPCI_ERROR_NOT_SUPPORTED);
 		}
 
 		/* Fall through to forward the call to the other world */
@@ -430,17 +475,8 @@
 		 * simply forward the call to the Normal world.
 		 */
 
-		/* Save incoming security state */
-		cm_el1_sysregs_context_save(in_sstate);
-
-		/* Restore outgoing security state */
-		cm_el1_sysregs_context_restore(out_sstate);
-		cm_set_next_eret_context(out_sstate);
-
-		SMC_RET8(cm_get_context(out_sstate), smc_fid, x1, x2, x3, x4,
-			 SMC_GET_GP(handle, CTX_GPREG_X5),
-			 SMC_GET_GP(handle, CTX_GPREG_X6),
-			 SMC_GET_GP(handle, CTX_GPREG_X7));
+		return spmd_smc_forward(smc_fid, secure_origin,
+					x1, x2, x3, x4, handle);
 		break; /* not reached */
 
 	case SPCI_MSG_WAIT:
@@ -449,39 +485,25 @@
 		 * this CPU from the Secure world. If so, then indicate that the
 		 * SPM core initialised successfully.
 		 */
-		if ((in_sstate == SECURE) && (ctx->state == SPMC_STATE_RESET)) {
+		if (secure_origin && (ctx->state == SPMC_STATE_RESET)) {
 			spmd_spm_core_sync_exit(0);
 		}
 
-		/* Intentional fall-through */
+		/* Fall through to forward the call to the other world */
 
 	case SPCI_MSG_YIELD:
 		/* This interface must be invoked only by the Secure world */
-		if (in_sstate == NON_SECURE) {
-			SMC_RET8(handle, SPCI_ERROR,
-				 SPCI_TARGET_INFO_MBZ, SPCI_ERROR_NOT_SUPPORTED,
-				 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ, SPCI_PARAM_MBZ,
-				 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ);
+		if (!secure_origin) {
+			return spmd_spci_error_return(handle,
+						      SPCI_ERROR_NOT_SUPPORTED);
 		}
 
-		/* Save incoming security state */
-		cm_el1_sysregs_context_save(in_sstate);
-
-		/* Restore outgoing security state */
-		cm_el1_sysregs_context_restore(out_sstate);
-		cm_set_next_eret_context(out_sstate);
-
-		SMC_RET8(cm_get_context(out_sstate), smc_fid, x1, x2, x3, x4,
-			 SMC_GET_GP(handle, CTX_GPREG_X5),
-			 SMC_GET_GP(handle, CTX_GPREG_X6),
-			 SMC_GET_GP(handle, CTX_GPREG_X7));
+		return spmd_smc_forward(smc_fid, secure_origin,
+					x1, x2, x3, x4, handle);
 		break; /* not reached */
 
 	default:
 		WARN("SPM: Unsupported call 0x%08x\n", smc_fid);
-		SMC_RET8(handle, SPCI_ERROR,
-			 SPCI_TARGET_INFO_MBZ, SPCI_ERROR_NOT_SUPPORTED,
-			 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ, SPCI_PARAM_MBZ,
-			 SPCI_PARAM_MBZ, SPCI_PARAM_MBZ);
+		return spmd_spci_error_return(handle, SPCI_ERROR_NOT_SUPPORTED);
 	}
 }