Merge "docs: fix mailing lists URLs" into integration
diff --git a/Makefile b/Makefile
index da3e493..3a8a522 100644
--- a/Makefile
+++ b/Makefile
@@ -523,6 +523,9 @@
             ifeq ($(CTX_INCLUDE_EL2_REGS),0)
                 $(error SPMD with SPM at S-EL2 requires CTX_INCLUDE_EL2_REGS option)
             endif
+	    ifeq ($(SPMC_AT_EL3),1)
+                $(error SPM cannot be enabled in both S-EL2 and EL3.)
+            endif
         endif
 
         ifeq ($(findstring optee_sp,$(ARM_SPMC_MANIFEST_DTS)),optee_sp)
@@ -573,6 +576,9 @@
 ifneq (${ARCH},aarch64)
 	$(error ENABLE_RME requires AArch64)
 endif
+ifeq ($(SPMC_AT_EL3),1)
+	$(error SPMC_AT_EL3 and ENABLE_RME cannot both be enabled.)
+endif
 include services/std_svc/rmmd/rmmd.mk
 $(warning "RME is an experimental feature")
 endif
@@ -1002,6 +1008,7 @@
         SEPARATE_NOBITS_REGION \
         SPIN_ON_BL1_EXIT \
         SPM_MM \
+        SPMC_AT_EL3 \
         SPMD_SPM_AT_SEL2 \
         TRUSTED_BOARD_BOOT \
         CRYPTO_SUPPORT \
@@ -1134,6 +1141,7 @@
         SPD_${SPD} \
         SPIN_ON_BL1_EXIT \
         SPM_MM \
+        SPMC_AT_EL3 \
         SPMD_SPM_AT_SEL2 \
         TRUSTED_BOARD_BOOT \
         CRYPTO_SUPPORT \
diff --git a/bl31/bl31.mk b/bl31/bl31.mk
index 25c7964..214cf2f 100644
--- a/bl31/bl31.mk
+++ b/bl31/bl31.mk
@@ -18,12 +18,21 @@
     $(error EL3_EXCEPTION_HANDLING must be 1 for SPM-MM support)
   else
     $(info Including SPM Management Mode (MM) makefile)
-    include services/std_svc/spm_mm/spm_mm.mk
+    include services/std_svc/spm/common/spm.mk
+    include services/std_svc/spm/spm_mm/spm_mm.mk
   endif
 endif
 
 include lib/extensions/amu/amu.mk
 include lib/mpmm/mpmm.mk
+
+ifeq (${SPMC_AT_EL3},1)
+  $(warning "EL3 SPMC is an experimental feature")
+  $(info Including EL3 SPMC makefile)
+  include services/std_svc/spm/common/spm.mk
+  include services/std_svc/spm/el3_spmc/spmc.mk
+endif
+
 include lib/psci/psci_lib.mk
 
 BL31_SOURCES		+=	bl31/bl31_main.c				\
@@ -40,6 +49,8 @@
 				services/std_svc/std_svc_setup.c		\
 				${PSCI_LIB_SOURCES}				\
 				${SPMD_SOURCES}					\
+				${SPM_MM_SOURCES}				\
+				${SPMC_SOURCES}					\
 				${SPM_SOURCES}
 
 ifeq (${DISABLE_MTPMU},1)
diff --git a/docs/components/secure-partition-manager.rst b/docs/components/secure-partition-manager.rst
index 1e46ec0..2eaae75 100644
--- a/docs/components/secure-partition-manager.rst
+++ b/docs/components/secure-partition-manager.rst
@@ -127,14 +127,18 @@
 
 This section explains the TF-A build options involved in building with
 support for an FF-A based SPM where the SPMD is located at EL3 and the
-SPMC located at S-EL1 or S-EL2:
+SPMC located at S-EL1, S-EL2 or EL3:
 
 - **SPD=spmd**: this option selects the SPMD component to relay the FF-A
   protocol from NWd to SWd back and forth. It is not possible to
   enable another Secure Payload Dispatcher when this option is chosen.
 - **SPMD_SPM_AT_SEL2**: this option adjusts the SPMC exception
-  level to being S-EL1 or S-EL2. It defaults to enabled (value 1) when
+  level to being at S-EL2. It defaults to enabled (value 1) when
   SPD=spmd is chosen.
+- **SPMC_AT_EL3**: this option adjusts the SPMC exception level to being
+  at EL3.
+- If neither **SPMD_SPM_AT_SEL2** or **SPMC_AT_EL3** are enabled the SPMC
+  exception level is set to S-EL1.
 - **CTX_INCLUDE_EL2_REGS**: this option permits saving (resp.
   restoring) the EL2 system register context before entering (resp.
   after leaving) the SPMC. It is mandatorily enabled when
@@ -146,14 +150,16 @@
   is required when ``SPMD_SPM_AT_SEL2`` is enabled hence when multiple
   secure partitions are to be loaded on behalf of the SPMC.
 
-+---------------+----------------------+------------------+
-|               | CTX_INCLUDE_EL2_REGS | SPMD_SPM_AT_SEL2 |
-+---------------+----------------------+------------------+
-| SPMC at S-EL1 |         0            |        0         |
-+---------------+----------------------+------------------+
-| SPMC at S-EL2 |         1            | 1 (default when  |
-|               |                      |    SPD=spmd)     |
-+---------------+----------------------+------------------+
++---------------+----------------------+------------------+-------------+
+|               | CTX_INCLUDE_EL2_REGS | SPMD_SPM_AT_SEL2 | SPMC_AT_EL3 |
++---------------+----------------------+------------------+-------------+
+| SPMC at S-EL1 |         0            |        0         |      0      |
++---------------+----------------------+------------------+-------------+
+| SPMC at S-EL2 |         1            | 1 (default when  |      0      |
+|               |                      |    SPD=spmd)     |             |
++---------------+----------------------+------------------+-------------+
+| SPMC at EL3   |         0            |        0         |      1      |
++---------------+----------------------+------------------+-------------+
 
 Other combinations of such build options either break the build or are not
 supported.
@@ -229,6 +235,20 @@
     GENERATE_COT=1 \
     all fip
 
+Sample TF-A build command line when SPMC is located at EL3:
+
+.. code:: shell
+
+    make \
+    CROSS_COMPILE=aarch64-none-elf- \
+    SPD=spmd \
+    SPMD_SPM_AT_SEL2=0 \
+    SPMC_AT_EL3=1 \
+    BL32=<path-to-tee-binary> \
+    BL33=<path-to-bl33-binary> \
+    PLAT=fvp \
+    all fip
+
 FVP model invocation
 ====================
 
diff --git a/docs/design/cpu-specific-build-macros.rst b/docs/design/cpu-specific-build-macros.rst
index af0e769..3029458 100644
--- a/docs/design/cpu-specific-build-macros.rst
+++ b/docs/design/cpu-specific-build-macros.rst
@@ -490,6 +490,10 @@
    Cortex-X2 CPU. This needs to be enabled only for revisions r0p0, r1p0 and
    r2p0 of the CPU, it is fixed in r2p1.
 
+-  ``ERRATA_X2_2147715``: This applies errata 2147715 workaround to
+   Cortex-X2 CPU. This needs to be enabled only for revision r2p0 of the CPU,
+   it is fixed in r2p1.
+
 For Cortex-A510, the following errata build flags are defined :
 
 -  ``ERRATA_A510_1922240``: This applies errata 1922240 workaround to
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 4dbf5cb..d30e22f 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -780,13 +780,20 @@
    firmware images have been loaded in memory, and the MMU and caches are
    turned off. Refer to the "Debugging options" section for more details.
 
+-  ``SPMC_AT_EL3`` : This boolean option is used jointly with the SPM
+   Dispatcher option (``SPD=spmd``). When enabled (1) it indicates the SPMC
+   component runs at the EL3 exception level. The default value is ``0`` (
+   disabled). This configuration supports pre-Armv8.4 platforms (aka not
+   implementing the ``FEAT_SEL2`` extension). This is an experimental feature.
+
 -  ``SPMD_SPM_AT_SEL2`` : This boolean option is used jointly with the SPM
    Dispatcher option (``SPD=spmd``). When enabled (1) it indicates the SPMC
-   component runs at the S-EL2 execution state provided by the Armv8.4-SecEL2
+   component runs at the S-EL2 exception level provided by the ``FEAT_SEL2``
    extension. This is the default when enabling the SPM Dispatcher. When
    disabled (0) it indicates the SPMC component runs at the S-EL1 execution
-   state. This latter configuration supports pre-Armv8.4 platforms (aka not
-   implementing the Armv8.4-SecEL2 extension).
+   state or at EL3 if ``SPMC_AT_EL3`` is enabled. The latter configurations
+   support pre-Armv8.4 platforms (aka not implementing the ``FEAT_SEL2``
+   extension).
 
 -  ``SPM_MM`` : Boolean option to enable the Management Mode (MM)-based Secure
    Partition Manager (SPM) implementation. The default value is ``0``
diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
index 3c27aff..5e3575f 100644
--- a/drivers/ufs/ufs.c
+++ b/drivers/ufs/ufs.c
@@ -656,7 +656,6 @@
 	sense_data_t *sense;
 	unsigned char data[CACHE_WRITEBACK_GRANULE << 1];
 	uintptr_t buf;
-	int result;
 	int retry;
 
 	assert((ufs_params.reg_base != 0) &&
@@ -693,14 +692,12 @@
 		/* logical block length in bytes */
 		*size = be32toh(*(unsigned int *)(buf + 4));
 	} while (retry);
-	(void)result;
 }
 
 size_t ufs_read_blocks(int lun, int lba, uintptr_t buf, size_t size)
 {
 	utp_utrd_t utrd;
 	resp_upiu_t *resp;
-	int result;
 
 	assert((ufs_params.reg_base != 0) &&
 	       (ufs_params.desc_base != 0) &&
@@ -711,7 +708,6 @@
 	dump_upiu(&utrd);
 #endif
 	resp = (resp_upiu_t *)utrd.resp_upiu;
-	(void)result;
 	return size - resp->res_trans_cnt;
 }
 
@@ -719,7 +715,6 @@
 {
 	utp_utrd_t utrd;
 	resp_upiu_t *resp;
-	int result;
 
 	assert((ufs_params.reg_base != 0) &&
 	       (ufs_params.desc_base != 0) &&
@@ -730,7 +725,6 @@
 	dump_upiu(&utrd);
 #endif
 	resp = (resp_upiu_t *)utrd.resp_upiu;
-	(void)result;
 	return size - resp->res_trans_cnt;
 }
 
diff --git a/fdts/stm32mp131.dtsi b/fdts/stm32mp131.dtsi
index dff1b33..adf7a91 100644
--- a/fdts/stm32mp131.dtsi
+++ b/fdts/stm32mp131.dtsi
@@ -26,26 +26,6 @@
 		};
 	};
 
-	nvmem_layout: nvmem_layout@0 {
-		compatible = "st,stm32-nvmem-layout";
-
-		nvmem-cells = <&cfg0_otp>,
-			      <&part_number_otp>,
-			      <&monotonic_otp>,
-			      <&nand_otp>,
-			      <&nand2_otp>,
-			      <&uid_otp>,
-			      <&hw2_otp>;
-
-		nvmem-cell-names = "cfg0_otp",
-				   "part_number_otp",
-				   "monotonic_otp",
-				   "nand_otp",
-				   "nand2_otp",
-				   "uid_otp",
-				   "hw2_otp";
-	};
-
 	clocks {
 		clk_csi: clk-csi {
 			#clock-cells = <0>;
diff --git a/fdts/stm32mp135f-dk.dts b/fdts/stm32mp135f-dk.dts
index 0fa064b..6240381 100644
--- a/fdts/stm32mp135f-dk.dts
+++ b/fdts/stm32mp135f-dk.dts
@@ -175,28 +175,6 @@
 	status = "okay";
 };
 
-&nvmem_layout {
-	nvmem-cells = <&cfg0_otp>,
-		      <&part_number_otp>,
-		      <&monotonic_otp>,
-		      <&nand_otp>,
-		      <&nand2_otp>,
-		      <&uid_otp>,
-		      <&hw2_otp>,
-		      <&pkh_otp>,
-		      <&board_id>;
-
-	nvmem-cell-names = "cfg0_otp",
-			   "part_number_otp",
-			   "monotonic_otp",
-			   "nand_otp",
-			   "nand2_otp",
-			   "uid_otp",
-			   "hw2_otp",
-			   "pkh_otp",
-			   "board_id";
-};
-
 &pka {
 	secure-status = "okay";
 };
diff --git a/fdts/stm32mp151.dtsi b/fdts/stm32mp151.dtsi
index 454e124..63cc917 100644
--- a/fdts/stm32mp151.dtsi
+++ b/fdts/stm32mp151.dtsi
@@ -24,26 +24,6 @@
 		};
 	};
 
-	nvmem_layout: nvmem_layout@0 {
-		compatible = "st,stm32-nvmem-layout";
-
-		nvmem-cells = <&cfg0_otp>,
-			      <&part_number_otp>,
-			      <&monotonic_otp>,
-			      <&nand_otp>,
-			      <&uid_otp>,
-			      <&package_otp>,
-			      <&hw2_otp>;
-
-		nvmem-cell-names = "cfg0_otp",
-				   "part_number_otp",
-				   "monotonic_otp",
-				   "nand_otp",
-				   "uid_otp",
-				   "package_otp",
-				   "hw2_otp";
-	};
-
 	psci {
 		compatible = "arm,psci-1.0";
 		method = "smc";
diff --git a/fdts/stm32mp157c-ed1.dts b/fdts/stm32mp157c-ed1.dts
index 44c7016..659e8bf 100644
--- a/fdts/stm32mp157c-ed1.dts
+++ b/fdts/stm32mp157c-ed1.dts
@@ -195,26 +195,6 @@
 	status = "okay";
 };
 
-&nvmem_layout {
-	nvmem-cells = <&cfg0_otp>,
-		      <&part_number_otp>,
-		      <&monotonic_otp>,
-		      <&nand_otp>,
-		      <&uid_otp>,
-		      <&package_otp>,
-		      <&hw2_otp>,
-		      <&board_id>;
-
-	nvmem-cell-names = "cfg0_otp",
-			   "part_number_otp",
-			   "monotonic_otp",
-			   "nand_otp",
-			   "uid_otp",
-			   "package_otp",
-			   "hw2_otp",
-			   "board_id";
-};
-
 &pwr_regulators {
 	vdd-supply = <&vdd>;
 	vdd_3v3_usbfs-supply = <&vdd_usb>;
diff --git a/fdts/stm32mp15xx-dkx.dtsi b/fdts/stm32mp15xx-dkx.dtsi
index 2eb3a57..05eb46a 100644
--- a/fdts/stm32mp15xx-dkx.dtsi
+++ b/fdts/stm32mp15xx-dkx.dtsi
@@ -183,26 +183,6 @@
 	secure-status = "okay";
 };
 
-&nvmem_layout {
-	nvmem-cells = <&cfg0_otp>,
-		      <&part_number_otp>,
-		      <&monotonic_otp>,
-		      <&nand_otp>,
-		      <&uid_otp>,
-		      <&package_otp>,
-		      <&hw2_otp>,
-		      <&board_id>;
-
-	nvmem-cell-names = "cfg0_otp",
-			   "part_number_otp",
-			   "monotonic_otp",
-			   "nand_otp",
-			   "uid_otp",
-			   "package_otp",
-			   "hw2_otp",
-			   "board_id";
-};
-
 &pwr_regulators {
 	vdd-supply = <&vdd>;
 	vdd_3v3_usbfs-supply = <&vdd_usb>;
diff --git a/include/lib/cpus/aarch64/cortex_x2.h b/include/lib/cpus/aarch64/cortex_x2.h
index 62530e2..92140b1 100644
--- a/include/lib/cpus/aarch64/cortex_x2.h
+++ b/include/lib/cpus/aarch64/cortex_x2.h
@@ -34,6 +34,12 @@
 #define CORTEX_X2_CPUPWRCTLR_EL1_CORE_PWRDN_BIT			U(1)
 
 /*******************************************************************************
+ * CPU Auxiliary Control Register definitions
+ ******************************************************************************/
+#define CORTEX_X2_CPUACTLR_EL1					S3_0_C15_C1_0
+#define CORTEX_X2_CPUACTLR_EL1_BIT_22				(ULL(1) << 22)
+
+/*******************************************************************************
  * CPU Auxiliary Control Register 5 definitions
  ******************************************************************************/
 #define CORTEX_X2_CPUACTLR5_EL1					S3_0_C15_C8_0
diff --git a/include/services/ffa_svc.h b/include/services/ffa_svc.h
index 9a7c489..d3fb012 100644
--- a/include/services/ffa_svc.h
+++ b/include/services/ffa_svc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020-2021, Arm Limited. All rights reserved.
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,8 @@
 #ifndef FFA_SVC_H
 #define FFA_SVC_H
 
+#include <stdbool.h>
+
 #include <lib/smccc.h>
 #include <lib/utils_def.h>
 #include <tools_share/uuid.h>
@@ -107,6 +109,7 @@
 #define FFA_VERSION		FFA_FID(SMC_32, FFA_FNUM_VERSION)
 #define FFA_FEATURES		FFA_FID(SMC_32, FFA_FNUM_FEATURES)
 #define FFA_RX_RELEASE		FFA_FID(SMC_32, FFA_FNUM_RX_RELEASE)
+#define FFA_RX_ACQUIRE		FFA_FID(SMC_32, FFA_FNUM_RX_ACQUIRE)
 #define FFA_RXTX_MAP_SMC32	FFA_FID(SMC_32, FFA_FNUM_RXTX_MAP)
 #define FFA_RXTX_UNMAP		FFA_FID(SMC_32, FFA_FNUM_RXTX_UNMAP)
 #define FFA_PARTITION_INFO_GET	FFA_FID(SMC_32, FFA_FNUM_PARTITION_INFO_GET)
@@ -176,6 +179,15 @@
 #define FFA_ENDPOINT_ID_MAX			U(1 << 16)
 
 /*
+ * Reserve endpoint id for the SPMD.
+ */
+#define SPMD_DIRECT_MSG_ENDPOINT_ID		U(FFA_ENDPOINT_ID_MAX - 1)
+
+/* Mask and shift to check valid secure FF-A Endpoint ID. */
+#define SPMC_SECURE_ID_MASK			U(1)
+#define SPMC_SECURE_ID_SHIFT			U(15)
+
+/*
  * Mask for source and destination endpoint id in
  * a direct message request/response.
  */
@@ -209,4 +221,24 @@
 		FFA_DIRECT_MSG_ENDPOINT_ID_MASK;
 }
 
+/******************************************************************************
+ * FF-A helper functions to determine partition ID world.
+ *****************************************************************************/
+
+/*
+ * Determine if provided ID is in the secure world.
+ */
+static inline bool ffa_is_secure_world_id(uint16_t id)
+{
+	return ((id >> SPMC_SECURE_ID_SHIFT) & SPMC_SECURE_ID_MASK) == 1;
+}
+
+/*
+ * Determine if provided ID is in the normal world.
+ */
+static inline bool ffa_is_normal_world_id(uint16_t id)
+{
+	return !ffa_is_secure_world_id(id);
+}
+
 #endif /* FFA_SVC_H */
diff --git a/include/services/spmc_svc.h b/include/services/spmc_svc.h
new file mode 100644
index 0000000..8ee61e9
--- /dev/null
+++ b/include/services/spmc_svc.h
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SPMC_SVC_H
+#define SPMC_SVC_H
+
+#ifndef __ASSEMBLER__
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+#include <services/ffa_svc.h>
+#include <services/spm_core_manifest.h>
+
+int spmc_setup(void);
+void spmc_populate_attrs(spmc_manifest_attribute_t *spmc_attrs);
+void *spmc_get_config_addr(void);
+
+void spmc_set_config_addr(uintptr_t soc_fw_config);
+
+uint64_t spmc_smc_handler(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint64_t x1,
+			  uint64_t x2,
+			  uint64_t x3,
+			  uint64_t x4,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags);
+
+static inline bool is_spmc_at_el3(void)
+{
+	return SPMC_AT_EL3 == 1;
+}
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* SPMC_SVC_H */
diff --git a/include/services/spmd_svc.h b/include/services/spmd_svc.h
index 1e7e6aa..29dfdad 100644
--- a/include/services/spmd_svc.h
+++ b/include/services/spmd_svc.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -12,6 +12,14 @@
 #include <stdint.h>
 
 int spmd_setup(void);
+uint64_t spmd_ffa_smc_handler(uint32_t smc_fid,
+			      uint64_t x1,
+			      uint64_t x2,
+			      uint64_t x3,
+			      uint64_t x4,
+			      void *cookie,
+			      void *handle,
+			      uint64_t flags);
 uint64_t spmd_smc_handler(uint32_t smc_fid,
 			  uint64_t x1,
 			  uint64_t x2,
@@ -20,6 +28,13 @@
 			  void *cookie,
 			  void *handle,
 			  uint64_t flags);
+uint64_t spmd_smc_switch_state(uint32_t smc_fid,
+			       bool secure_origin,
+			       uint64_t x1,
+			       uint64_t x2,
+			       uint64_t x3,
+			       uint64_t x4,
+			       void *handle);
 #endif /* __ASSEMBLER__ */
 
 #endif /* SPMD_SVC_H */
diff --git a/lib/cpus/aarch64/cortex_x2.S b/lib/cpus/aarch64/cortex_x2.S
index 9586a5b..90a906b 100644
--- a/lib/cpus/aarch64/cortex_x2.S
+++ b/lib/cpus/aarch64/cortex_x2.S
@@ -237,6 +237,36 @@
 	ret
 endfunc check_errata_cve_2022_23960
 
+	/* ---------------------------------------------------------
+	 * Errata Workaround for Cortex-X2 Errata 2147715.
+	 * This applies only to revisions r2p0 and is fixed in r2p1.
+	 * Inputs:
+	 * x0: variant[4:7] and revision[0:3] of current cpu.
+	 * Shall clobber: x0, x1, x17
+	 * ---------------------------------------------------------
+	 */
+func errata_x2_2147715_wa
+	/* Compare x0 against revision r2p0 */
+	mov     x17, x30
+	bl      check_errata_2147715
+	cbz     x0, 1f
+
+	/* Apply the workaround by setting bit 22 in CPUACTLR_EL1. */
+	mrs     x1, CORTEX_X2_CPUACTLR_EL1
+	orr     x1, x1, CORTEX_X2_CPUACTLR_EL1_BIT_22
+	msr     CORTEX_X2_CPUACTLR_EL1, x1
+
+1:
+	ret     x17
+endfunc errata_x2_2147715_wa
+
+func check_errata_2147715
+	/* Applies to r2p0 */
+	mov	x1, #0x20
+	mov	x2, #0x20
+	b	cpu_rev_var_range
+endfunc check_errata_2147715
+
 	/* ----------------------------------------------------
 	 * HW will do the cache maintenance while powering down
 	 * ----------------------------------------------------
@@ -268,10 +298,11 @@
 	 * checking functions of each errata.
 	 */
 	report_errata ERRATA_X2_2002765, cortex_x2, 2002765
-	report_errata ERRATA_X2_2058056, cortex_x2, 2058056
-	report_errata ERRATA_X2_2083908, cortex_x2, 2083908
 	report_errata ERRATA_X2_2017096, cortex_x2, 2017096
+	report_errata ERRATA_X2_2058056, cortex_x2, 2058056
 	report_errata ERRATA_X2_2081180, cortex_x2, 2081180
+	report_errata ERRATA_X2_2083908, cortex_x2, 2083908
+	report_errata ERRATA_X2_2147715, cortex_x2, 2147715
 	report_errata ERRATA_X2_2216384, cortex_x2, 2216384
 	report_errata WORKAROUND_CVE_2022_23960, cortex_x2, cve_2022_23960
 
@@ -321,6 +352,11 @@
 	bl	errata_x2_2216384_wa
 #endif
 
+#if ERRATA_X2_2147715
+	mov	x0, x18
+	bl	errata_x2_2147715_wa
+#endif
+
 #if IMAGE_BL31 && WORKAROUND_CVE_2022_23960
 	/*
 	 * The Cortex-X2 generic vectors are overridden to apply errata
diff --git a/lib/cpus/cpu-ops.mk b/lib/cpus/cpu-ops.mk
index 8840f8e..462ca9d 100644
--- a/lib/cpus/cpu-ops.mk
+++ b/lib/cpus/cpu-ops.mk
@@ -547,6 +547,10 @@
 # r2p1.
 ERRATA_X2_2216384	?=0
 
+# Flag to apply erratum 2147715 workaround during reset. This erratum applies
+# only to revision r2p0 of the Cortex-X2 cpu, it is fixed in r2p1.
+ERRATA_X2_2147715	?=0
+
 # Flag to apply erratum 1922240 workaround during reset. This erratum applies
 # to revision r0p0 of the Cortex-A510 cpu and is fixed in r0p1.
 ERRATA_A510_1922240	?=0
@@ -1046,6 +1050,10 @@
 $(eval $(call assert_boolean,ERRATA_X2_2216384))
 $(eval $(call add_define,ERRATA_X2_2216384))
 
+# Process ERRATA_X2_2147715 flag
+$(eval $(call assert_boolean,ERRATA_X2_2147715))
+$(eval $(call add_define,ERRATA_X2_2147715))
+
 # Process ERRATA_A510_1922240 flag
 $(eval $(call assert_boolean,ERRATA_A510_1922240))
 $(eval $(call add_define,ERRATA_A510_1922240))
diff --git a/lib/xlat_tables_v2/ro_xlat_tables.mk b/lib/xlat_tables_v2/ro_xlat_tables.mk
index 7991e1a..fb8a426 100644
--- a/lib/xlat_tables_v2/ro_xlat_tables.mk
+++ b/lib/xlat_tables_v2/ro_xlat_tables.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2020, ARM Limited. All rights reserved.
+# Copyright (c) 2020-2022, ARM Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -34,4 +34,8 @@
                  attributes, which is not possible once the translation tables \
                  have been made read-only.")
     endif
+    ifeq (${SPMC_AT_EL3},1)
+        $(error "EL3 SPMC requires functionality from the dynamic translation \
+                 library and is incompatible with ALLOW_RO_XLAT_TABLES.")
+    endif
 endif
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index 99f44a4..7b66569 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -288,6 +288,9 @@
 # Enable the Management Mode (MM)-based Secure Partition Manager implementation
 SPM_MM				:= 0
 
+# Use the FF-A SPMC implementation in EL3.
+SPMC_AT_EL3			:= 0
+
 # Use SPM at S-EL2 as a default config for SPMD
 SPMD_SPM_AT_SEL2		:= 1
 
diff --git a/plat/st/common/stm32mp_dt.c b/plat/st/common/stm32mp_dt.c
index ea71571..c9efeb5 100644
--- a/plat/st/common/stm32mp_dt.c
+++ b/plat/st/common/stm32mp_dt.c
@@ -11,8 +11,6 @@
 #include <common/fdt_wrappers.h>
 #include <drivers/st/regulator.h>
 #include <drivers/st/stm32_gpio.h>
-#include <drivers/st/stm32mp1_ddr.h>
-#include <drivers/st/stm32mp1_ram.h>
 #include <libfdt.h>
 
 #include <platform_def.h>
@@ -328,48 +326,32 @@
 int dt_find_otp_name(const char *name, uint32_t *otp, uint32_t *otp_len)
 {
 	int node;
-	int index, len;
+	int len;
 	const fdt32_t *cuint;
 
 	if ((name == NULL) || (otp == NULL)) {
 		return -FDT_ERR_BADVALUE;
 	}
 
-	node = fdt_node_offset_by_compatible(fdt, -1, DT_NVMEM_LAYOUT_COMPAT);
+	node = fdt_node_offset_by_compatible(fdt, -1, DT_BSEC_COMPAT);
 	if (node < 0) {
 		return node;
 	}
 
-	index = fdt_stringlist_search(fdt, node, "nvmem-cell-names", name);
-	if (index < 0) {
-		return index;
-	}
-
-	cuint = fdt_getprop(fdt, node, "nvmem-cells", &len);
-	if (cuint == NULL) {
-		return -FDT_ERR_NOTFOUND;
-	}
-
-	if ((index * (int)sizeof(uint32_t)) > len) {
-		return -FDT_ERR_BADVALUE;
-	}
-
-	cuint += index;
-
-	node = fdt_node_offset_by_phandle(fdt, fdt32_to_cpu(*cuint));
+	node = fdt_subnode_offset(fdt, node, name);
 	if (node < 0) {
-		ERROR("Malformed nvmem_layout node: ignored\n");
+		ERROR("nvmem node %s not found\n", name);
 		return node;
 	}
 
 	cuint = fdt_getprop(fdt, node, "reg", &len);
 	if ((cuint == NULL) || (len != (2 * (int)sizeof(uint32_t)))) {
-		ERROR("Malformed nvmem_layout node: ignored\n");
+		ERROR("Malformed nvmem node %s: ignored\n", name);
 		return -FDT_ERR_BADVALUE;
 	}
 
 	if (fdt32_to_cpu(*cuint) % sizeof(uint32_t)) {
-		ERROR("Misaligned nvmem_layout element: ignored\n");
+		ERROR("Misaligned nvmem %s element: ignored\n", name);
 		return -FDT_ERR_BADVALUE;
 	}
 
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index a890534..9e67989 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -146,7 +146,7 @@
 endif
 
 $(eval DTC_V = $(shell $(DTC) -v | awk '{print $$NF}'))
-$(eval DTC_VERSION = $(shell printf "%d" $(shell echo ${DTC_V} | cut -d- -f1 | sed "s/\./0/g" | grep -o [0-9]*)))
+$(eval DTC_VERSION = $(shell printf "%d" $(shell echo ${DTC_V} | cut -d- -f1 | sed "s/\./0/g" | grep -o "[0-9]*")))
 DTC_CPPFLAGS		+=	${INCLUDES}
 DTC_FLAGS		+=	-Wno-unit_address_vs_reg
 ifeq ($(shell test $(DTC_VERSION) -ge 10601; echo $$?),0)
diff --git a/plat/st/stm32mp1/stm32mp1_def.h b/plat/st/stm32mp1/stm32mp1_def.h
index d869978..017e3b1 100644
--- a/plat/st/stm32mp1/stm32mp1_def.h
+++ b/plat/st/stm32mp1/stm32mp1_def.h
@@ -621,7 +621,6 @@
 #define DT_DDR_COMPAT			"st,stm32mp1-ddr"
 #endif
 #define DT_IWDG_COMPAT			"st,stm32mp1-iwdg"
-#define DT_NVMEM_LAYOUT_COMPAT		"st,stm32-nvmem-layout"
 #define DT_PWR_COMPAT			"st,stm32mp1,pwr-reg"
 #if STM32MP13
 #define DT_RCC_CLK_COMPAT		"st,stm32mp13-rcc"
diff --git a/services/std_svc/spm_mm/aarch64/spm_mm_helpers.S b/services/std_svc/spm/common/aarch64/spm_helpers.S
similarity index 96%
rename from services/std_svc/spm_mm/aarch64/spm_mm_helpers.S
rename to services/std_svc/spm/common/aarch64/spm_helpers.S
index 2c3aaf7..95e69fb 100644
--- a/services/std_svc/spm_mm/aarch64/spm_mm_helpers.S
+++ b/services/std_svc/spm/common/aarch64/spm_helpers.S
@@ -1,11 +1,11 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <asm_macros.S>
-#include "../spm_mm_private.h"
+#include "spm_common.h"
 
 	.global spm_secure_partition_enter
 	.global spm_secure_partition_exit
diff --git a/services/std_svc/spm/common/include/spm_common.h b/services/std_svc/spm/common/include/spm_common.h
new file mode 100644
index 0000000..68805fc
--- /dev/null
+++ b/services/std_svc/spm/common/include/spm_common.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SPM_COMMON_H
+#define SPM_COMMON_H
+
+#include <context.h>
+
+/*******************************************************************************
+ * Constants that allow assembler code to preserve callee-saved registers of the
+ * C runtime context while performing a security state switch.
+ ******************************************************************************/
+#define SP_C_RT_CTX_X19		0x0
+#define SP_C_RT_CTX_X20		0x8
+#define SP_C_RT_CTX_X21		0x10
+#define SP_C_RT_CTX_X22		0x18
+#define SP_C_RT_CTX_X23		0x20
+#define SP_C_RT_CTX_X24		0x28
+#define SP_C_RT_CTX_X25		0x30
+#define SP_C_RT_CTX_X26		0x38
+#define SP_C_RT_CTX_X27		0x40
+#define SP_C_RT_CTX_X28		0x48
+#define SP_C_RT_CTX_X29		0x50
+#define SP_C_RT_CTX_X30		0x58
+
+#define SP_C_RT_CTX_SIZE	0x60
+#define SP_C_RT_CTX_ENTRIES	(SP_C_RT_CTX_SIZE >> DWORD_SHIFT)
+
+#ifndef __ASSEMBLER__
+
+#include <stdint.h>
+
+/* Assembly helpers */
+uint64_t spm_secure_partition_enter(uint64_t *c_rt_ctx);
+void __dead2 spm_secure_partition_exit(uint64_t c_rt_ctx, uint64_t ret);
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* SPM_COMMON_H */
diff --git a/services/std_svc/spm/common/spm.mk b/services/std_svc/spm/common/spm.mk
new file mode 100644
index 0000000..9aa96be
--- /dev/null
+++ b/services/std_svc/spm/common/spm.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifneq (${ARCH},aarch64)
+        $(error "Error: SPM is only supported on aarch64.")
+endif
+
+INCLUDES	+=	-Iservices/std_svc/spm/common/include
+
+SPM_SOURCES	:=	$(addprefix services/std_svc/spm/common/,\
+			${ARCH}/spm_helpers.S)
+
+# Let the top-level Makefile know that we intend to include a BL32 image
+NEED_BL32		:=	yes
diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
new file mode 100644
index 0000000..df0aa61
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc.h
@@ -0,0 +1,187 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SPMC_H
+#define SPMC_H
+
+#include <stdint.h>
+
+#include <lib/psci/psci.h>
+#include <lib/spinlock.h>
+#include "spm_common.h"
+
+/*
+ * Ranges of FF-A IDs for Normal world and Secure world components. The
+ * convention matches that used by other SPMCs i.e. Hafnium and OP-TEE.
+ */
+#define FFA_NWD_ID_BASE		0x0
+#define FFA_NWD_ID_LIMIT	0x7FFF
+#define FFA_SWD_ID_BASE		0x8000
+#define FFA_SWD_ID_LIMIT	SPMD_DIRECT_MSG_ENDPOINT_ID - 1
+#define FFA_SWD_ID_MASK		0x8000
+
+/* ID 0 is reserved for the normal world entity, (Hypervisor or OS Kernel). */
+#define FFA_NWD_ID		U(0)
+/* First ID is reserved for the SPMC */
+#define FFA_SPMC_ID		U(FFA_SWD_ID_BASE)
+/* SP IDs are allocated after the SPMC ID */
+#define FFA_SP_ID_BASE		(FFA_SPMC_ID + 1)
+/* Align with Hafnium implementation */
+#define INV_SP_ID		0x7FFF
+
+/* FF-A warm boot types. */
+#define FFA_WB_TYPE_S2RAM	0
+#define FFA_WB_TYPE_NOTS2RAM	1
+
+/*
+ * Runtime states of an execution context as per the FF-A v1.1 specification.
+ */
+enum sp_runtime_states {
+	RT_STATE_WAITING,
+	RT_STATE_RUNNING,
+	RT_STATE_PREEMPTED,
+	RT_STATE_BLOCKED
+};
+
+/*
+ * Runtime model of an execution context as per the FF-A v1.1 specification. Its
+ * value is valid only if the execution context is not in the waiting state.
+ */
+enum sp_runtime_model {
+	RT_MODEL_DIR_REQ,
+	RT_MODEL_RUN,
+	RT_MODEL_INIT,
+	RT_MODEL_INTR
+};
+
+enum sp_runtime_el {
+	EL1 = 0,
+	S_EL0,
+	S_EL1
+};
+
+enum sp_execution_state {
+	SP_STATE_AARCH64 = 0,
+	SP_STATE_AARCH32
+};
+
+/*
+ * Execution context members for an SP. This is a bit like struct
+ * vcpu in a hypervisor.
+ */
+struct sp_exec_ctx {
+	/*
+	 * Store the stack address to restore C runtime context from after
+	 * returning from a synchronous entry into the SP.
+	 */
+	uint64_t c_rt_ctx;
+
+	/* Space to maintain the architectural state of an SP. */
+	cpu_context_t cpu_ctx;
+
+	/* Track the current runtime state of the SP. */
+	enum sp_runtime_states rt_state;
+
+	/* Track the current runtime model of the SP. */
+	enum sp_runtime_model rt_model;
+};
+
+/*
+ * Structure to describe the cumulative properties of an SP.
+ */
+struct secure_partition_desc {
+	/*
+	 * Execution contexts allocated to this endpoint. Ideally,
+	 * we need as many contexts as there are physical cpus only
+	 * for a S-EL1 SP which is MP-pinned.
+	 */
+	struct sp_exec_ctx ec[PLATFORM_CORE_COUNT];
+
+	/* ID of the Secure Partition. */
+	uint16_t sp_id;
+
+	/* Runtime EL. */
+	enum sp_runtime_el runtime_el;
+
+	/* Partition UUID. */
+	uint32_t uuid[4];
+
+	/* Partition Properties. */
+	uint32_t properties;
+
+	/* Supported FF-A Version. */
+	uint32_t ffa_version;
+
+	/* Execution State. */
+	enum sp_execution_state execution_state;
+
+	/* Secondary entrypoint. Only valid for a S-EL1 SP. */
+	uintptr_t secondary_ep;
+};
+
+/*
+ * This define identifies the only SP that will be initialised and participate
+ * in FF-A communication. The implementation leaves the door open for more SPs
+ * to be managed in future but for now it is reasonable to assume that either a
+ * single S-EL0 or a single S-EL1 SP will be supported. This define will be used
+ * to identify which SP descriptor to initialise and manage during SP runtime.
+ */
+#define ACTIVE_SP_DESC_INDEX	0
+
+/*
+ * Structure to describe the cumulative properties of the Hypervisor and
+ * NS-Endpoints.
+ */
+struct ns_endpoint_desc {
+	/*
+	 * ID of the NS-Endpoint or Hypervisor.
+	 */
+	uint16_t ns_ep_id;
+
+	/*
+	 * Supported FF-A Version.
+	 */
+	uint32_t ffa_version;
+};
+
+/* Setup Function for different SP types. */
+void spmc_sp_common_setup(struct secure_partition_desc *sp,
+			  entry_point_info_t *ep_info);
+void spmc_el1_sp_setup(struct secure_partition_desc *sp,
+		       entry_point_info_t *ep_info);
+void spmc_sp_common_ep_commit(struct secure_partition_desc *sp,
+			      entry_point_info_t *ep_info);
+
+/*
+ * Helper function to perform a synchronous entry into a SP.
+ */
+uint64_t spmc_sp_synchronous_entry(struct sp_exec_ctx *ec);
+
+/*
+ * Helper function to obtain the descriptor of the current SP on a physical cpu.
+ */
+struct secure_partition_desc *spmc_get_current_sp_ctx(void);
+
+/*
+ * Helper function to obtain the execution context of an SP on a
+ * physical cpu.
+ */
+struct sp_exec_ctx *spmc_get_sp_ec(struct secure_partition_desc *sp);
+
+/*
+ * Helper function to obtain the index of the execution context of an SP on a
+ * physical cpu.
+ */
+unsigned int get_ec_index(struct secure_partition_desc *sp);
+
+uint64_t spmc_ffa_error_return(void *handle, int error_code);
+
+/*
+ * Ensure a partition ID does not clash and follows the secure world convention.
+ */
+bool is_ffa_secure_id_valid(uint16_t partition_id);
+
+#endif /* SPMC_H */
diff --git a/services/std_svc/spm/el3_spmc/spmc.mk b/services/std_svc/spm/el3_spmc/spmc.mk
new file mode 100644
index 0000000..2b154dd
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc.mk
@@ -0,0 +1,17 @@
+#
+# Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+ifneq (${ARCH},aarch64)
+        $(error "Error: SPMC is only supported on aarch64.")
+endif
+
+SPMC_SOURCES	:=	$(addprefix services/std_svc/spm/el3_spmc/,	\
+			spmc_main.c				\
+			spmc_setup.c)
+
+
+# Let the top-level Makefile know that we intend to include a BL32 image
+NEED_BL32		:=	yes
diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
new file mode 100644
index 0000000..3fd8c78
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c
@@ -0,0 +1,788 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <arch_helpers.h>
+#include <bl31/bl31.h>
+#include <bl31/ehf.h>
+#include <common/debug.h>
+#include <common/fdt_wrappers.h>
+#include <common/runtime_svc.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/smccc.h>
+#include <lib/utils.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+#include <services/ffa_svc.h>
+#include <services/spmc_svc.h>
+#include <services/spmd_svc.h>
+#include "spmc.h"
+
+#include <platform_def.h>
+
+/*
+ * Allocate a secure partition descriptor to describe each SP in the system that
+ * does not reside at EL3.
+ */
+static struct secure_partition_desc sp_desc[SECURE_PARTITION_COUNT];
+
+/*
+ * Allocate an NS endpoint descriptor to describe each VM and the Hypervisor in
+ * the system that interacts with a SP. It is used to track the Hypervisor
+ * buffer pair, version and ID for now. It could be extended to track VM
+ * properties when the SPMC supports indirect messaging.
+ */
+static struct ns_endpoint_desc ns_ep_desc[NS_PARTITION_COUNT];
+
+/*
+ * Helper function to obtain the descriptor of the last SP to whom control was
+ * handed to on this physical cpu. Currently, we assume there is only one SP.
+ * TODO: Expand to track multiple partitions when required.
+ */
+struct secure_partition_desc *spmc_get_current_sp_ctx(void)
+{
+	return &(sp_desc[ACTIVE_SP_DESC_INDEX]);
+}
+
+/*
+ * Helper function to obtain the execution context of an SP on the
+ * current physical cpu.
+ */
+struct sp_exec_ctx *spmc_get_sp_ec(struct secure_partition_desc *sp)
+{
+	return &(sp->ec[get_ec_index(sp)]);
+}
+
+/* Helper function to get pointer to SP context from its ID. */
+struct secure_partition_desc *spmc_get_sp_ctx(uint16_t id)
+{
+	/* Check for SWd Partitions. */
+	for (unsigned int i = 0U; i < SECURE_PARTITION_COUNT; i++) {
+		if (sp_desc[i].sp_id == id) {
+			return &(sp_desc[i]);
+		}
+	}
+	return NULL;
+}
+
+/******************************************************************************
+ * This function returns to the place where spmc_sp_synchronous_entry() was
+ * called originally.
+ ******************************************************************************/
+__dead2 void spmc_sp_synchronous_exit(struct sp_exec_ctx *ec, uint64_t rc)
+{
+	/*
+	 * The SPM must have initiated the original request through a
+	 * synchronous entry into the secure partition. Jump back to the
+	 * original C runtime context with the value of rc in x0;
+	 */
+	spm_secure_partition_exit(ec->c_rt_ctx, rc);
+
+	panic();
+}
+
+/*******************************************************************************
+ * Return FFA_ERROR with specified error code.
+ ******************************************************************************/
+uint64_t spmc_ffa_error_return(void *handle, int error_code)
+{
+	SMC_RET8(handle, FFA_ERROR,
+		 FFA_TARGET_INFO_MBZ, error_code,
+		 FFA_PARAM_MBZ, FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+		 FFA_PARAM_MBZ, FFA_PARAM_MBZ);
+}
+
+/******************************************************************************
+ * Helper function to validate a secure partition ID to ensure it does not
+ * conflict with any other FF-A component and follows the convention to
+ * indicate it resides within the secure world.
+ ******************************************************************************/
+bool is_ffa_secure_id_valid(uint16_t partition_id)
+{
+	/* Ensure the ID is not the invalid partition ID. */
+	if (partition_id == INV_SP_ID) {
+		return false;
+	}
+
+	/* Ensure the ID is not the SPMD ID. */
+	if (partition_id == SPMD_DIRECT_MSG_ENDPOINT_ID) {
+		return false;
+	}
+
+	/*
+	 * Ensure the ID follows the convention to indicate it resides
+	 * in the secure world.
+	 */
+	if (!ffa_is_secure_world_id(partition_id)) {
+		return false;
+	}
+
+	/* Ensure we don't conflict with the SPMC partition ID. */
+	if (partition_id == FFA_SPMC_ID) {
+		return false;
+	}
+
+	/* Ensure we do not already have an SP context with this ID. */
+	if (spmc_get_sp_ctx(partition_id)) {
+		return false;
+	}
+
+	return true;
+}
+
+/*******************************************************************************
+ * This function either forwards the request to the other world or returns
+ * with an ERET depending on the source of the call.
+ ******************************************************************************/
+static uint64_t spmc_smc_return(uint32_t smc_fid,
+				bool secure_origin,
+				uint64_t x1,
+				uint64_t x2,
+				uint64_t x3,
+				uint64_t x4,
+				void *handle,
+				void *cookie,
+				uint64_t flags,
+				uint16_t dst_id)
+{
+	/* If the destination is in the normal world always go via the SPMD. */
+	if (ffa_is_normal_world_id(dst_id)) {
+		return spmd_smc_handler(smc_fid, x1, x2, x3, x4,
+					cookie, handle, flags);
+	}
+	/*
+	 * If the caller is secure and we want to return to the secure world,
+	 * ERET directly.
+	 */
+	else if (secure_origin && ffa_is_secure_world_id(dst_id)) {
+		SMC_RET5(handle, smc_fid, x1, x2, x3, x4);
+	}
+	/* If we originated in the normal world then switch contexts. */
+	else if (!secure_origin && ffa_is_secure_world_id(dst_id)) {
+		return spmd_smc_switch_state(smc_fid, secure_origin, x1, x2,
+					     x3, x4, handle);
+	} else {
+		/* Unknown State. */
+		panic();
+	}
+
+	/* Shouldn't be Reached. */
+	return 0;
+}
+
+/*******************************************************************************
+ * FF-A ABI Handlers.
+ ******************************************************************************/
+
+/*******************************************************************************
+ * Helper function to validate arg2 as part of a direct message.
+ ******************************************************************************/
+static inline bool direct_msg_validate_arg2(uint64_t x2)
+{
+	/*
+	 * We currently only support partition messages, therefore ensure x2 is
+	 * not set.
+	 */
+	if (x2 != (uint64_t) 0) {
+		VERBOSE("Arg2 MBZ for partition messages (0x%lx).\n", x2);
+		return false;
+	}
+	return true;
+}
+
+/*******************************************************************************
+ * Handle direct request messages and route to the appropriate destination.
+ ******************************************************************************/
+static uint64_t direct_req_smc_handler(uint32_t smc_fid,
+				       bool secure_origin,
+				       uint64_t x1,
+				       uint64_t x2,
+				       uint64_t x3,
+				       uint64_t x4,
+				       void *cookie,
+				       void *handle,
+				       uint64_t flags)
+{
+	uint16_t dst_id = ffa_endpoint_destination(x1);
+	struct secure_partition_desc *sp;
+	unsigned int idx;
+
+	/* Check if arg2 has been populated correctly based on message type. */
+	if (!direct_msg_validate_arg2(x2)) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * If called by the secure world it is an invalid call since a
+	 * SP cannot call into the Normal world and there is no other SP to call
+	 * into. If there are other SPs in future then the partition runtime
+	 * model would need to be validated as well.
+	 */
+	if (secure_origin) {
+		VERBOSE("Direct request not supported to the Normal World.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Check if the SP ID is valid. */
+	sp = spmc_get_sp_ctx(dst_id);
+	if (sp == NULL) {
+		VERBOSE("Direct request to unknown partition ID (0x%x).\n",
+			dst_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Check that the target execution context is in a waiting state before
+	 * forwarding the direct request to it.
+	 */
+	idx = get_ec_index(sp);
+	if (sp->ec[idx].rt_state != RT_STATE_WAITING) {
+		VERBOSE("SP context on core%u is not waiting (%u).\n",
+			idx, sp->ec[idx].rt_model);
+		return spmc_ffa_error_return(handle, FFA_ERROR_BUSY);
+	}
+
+	/*
+	 * Everything checks out so forward the request to the SP after updating
+	 * its state and runtime model.
+	 */
+	sp->ec[idx].rt_state = RT_STATE_RUNNING;
+	sp->ec[idx].rt_model = RT_MODEL_DIR_REQ;
+	return spmc_smc_return(smc_fid, secure_origin, x1, x2, x3, x4,
+			       handle, cookie, flags, dst_id);
+}
+
+/*******************************************************************************
+ * Handle direct response messages and route to the appropriate destination.
+ ******************************************************************************/
+static uint64_t direct_resp_smc_handler(uint32_t smc_fid,
+					bool secure_origin,
+					uint64_t x1,
+					uint64_t x2,
+					uint64_t x3,
+					uint64_t x4,
+					void *cookie,
+					void *handle,
+					uint64_t flags)
+{
+	uint16_t dst_id = ffa_endpoint_destination(x1);
+	struct secure_partition_desc *sp;
+	unsigned int idx;
+
+	/* Check if arg2 has been populated correctly based on message type. */
+	if (!direct_msg_validate_arg2(x2)) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Check that the response did not originate from the Normal world. */
+	if (!secure_origin) {
+		VERBOSE("Direct Response not supported from Normal World.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Check that the response is either targeted to the Normal world or the
+	 * SPMC e.g. a PM response.
+	 */
+	if ((dst_id != FFA_SPMC_ID) && ffa_is_secure_world_id(dst_id)) {
+		VERBOSE("Direct response to invalid partition ID (0x%x).\n",
+			dst_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Obtain the SP descriptor and update its runtime state. */
+	sp = spmc_get_sp_ctx(ffa_endpoint_source(x1));
+	if (sp == NULL) {
+		VERBOSE("Direct response to unknown partition ID (0x%x).\n",
+			dst_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Sanity check state is being tracked correctly in the SPMC. */
+	idx = get_ec_index(sp);
+	assert(sp->ec[idx].rt_state == RT_STATE_RUNNING);
+
+	/* Ensure SP execution context was in the right runtime model. */
+	if (sp->ec[idx].rt_model != RT_MODEL_DIR_REQ) {
+		VERBOSE("SP context on core%u not handling direct req (%u).\n",
+			idx, sp->ec[idx].rt_model);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	/* Update the state of the SP execution context. */
+	sp->ec[idx].rt_state = RT_STATE_WAITING;
+
+	/*
+	 * If the receiver is not the SPMC then forward the response to the
+	 * Normal world.
+	 */
+	if (dst_id == FFA_SPMC_ID) {
+		spmc_sp_synchronous_exit(&sp->ec[idx], x4);
+		/* Should not get here. */
+		panic();
+	}
+
+	return spmc_smc_return(smc_fid, secure_origin, x1, x2, x3, x4,
+			       handle, cookie, flags, dst_id);
+}
+
+/*******************************************************************************
+ * This function handles the FFA_MSG_WAIT SMC to allow an SP to relinquish its
+ * cycles.
+ ******************************************************************************/
+static uint64_t msg_wait_handler(uint32_t smc_fid,
+				 bool secure_origin,
+				 uint64_t x1,
+				 uint64_t x2,
+				 uint64_t x3,
+				 uint64_t x4,
+				 void *cookie,
+				 void *handle,
+				 uint64_t flags)
+{
+	struct secure_partition_desc *sp;
+	unsigned int idx;
+
+	/*
+	 * Check that the response did not originate from the Normal world as
+	 * only the secure world can call this ABI.
+	 */
+	if (!secure_origin) {
+		VERBOSE("Normal world cannot call FFA_MSG_WAIT.\n");
+		return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/* Get the descriptor of the SP that invoked FFA_MSG_WAIT. */
+	sp = spmc_get_current_sp_ctx();
+	if (sp == NULL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Get the execution context of the SP that invoked FFA_MSG_WAIT.
+	 */
+	idx = get_ec_index(sp);
+
+	/* Ensure SP execution context was in the right runtime model. */
+	if (sp->ec[idx].rt_model == RT_MODEL_DIR_REQ) {
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	/* Sanity check the state is being tracked correctly in the SPMC. */
+	assert(sp->ec[idx].rt_state == RT_STATE_RUNNING);
+
+	/*
+	 * Perform a synchronous exit if the partition was initialising. The
+	 * state is updated after the exit.
+	 */
+	if (sp->ec[idx].rt_model == RT_MODEL_INIT) {
+		spmc_sp_synchronous_exit(&sp->ec[idx], x4);
+		/* Should not get here */
+		panic();
+	}
+
+	/* Update the state of the SP execution context. */
+	sp->ec[idx].rt_state = RT_STATE_WAITING;
+
+	/* Resume normal world if a secure interrupt was handled. */
+	if (sp->ec[idx].rt_model == RT_MODEL_INTR) {
+		/* FFA_MSG_WAIT can only be called from the secure world. */
+		unsigned int secure_state_in = SECURE;
+		unsigned int secure_state_out = NON_SECURE;
+
+		cm_el1_sysregs_context_save(secure_state_in);
+		cm_el1_sysregs_context_restore(secure_state_out);
+		cm_set_next_eret_context(secure_state_out);
+		SMC_RET0(cm_get_context(secure_state_out));
+	}
+
+	/* Forward the response to the Normal world. */
+	return spmc_smc_return(smc_fid, secure_origin, x1, x2, x3, x4,
+			       handle, cookie, flags, FFA_NWD_ID);
+}
+
+static uint64_t ffa_error_handler(uint32_t smc_fid,
+				 bool secure_origin,
+				 uint64_t x1,
+				 uint64_t x2,
+				 uint64_t x3,
+				 uint64_t x4,
+				 void *cookie,
+				 void *handle,
+				 uint64_t flags)
+{
+	struct secure_partition_desc *sp;
+	unsigned int idx;
+
+	/* Check that the response did not originate from the Normal world. */
+	if (!secure_origin) {
+		return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/* Get the descriptor of the SP that invoked FFA_ERROR. */
+	sp = spmc_get_current_sp_ctx();
+	if (sp == NULL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Get the execution context of the SP that invoked FFA_ERROR. */
+	idx = get_ec_index(sp);
+
+	/*
+	 * We only expect FFA_ERROR to be received during SP initialisation
+	 * otherwise this is an invalid call.
+	 */
+	if (sp->ec[idx].rt_model == RT_MODEL_INIT) {
+		ERROR("SP 0x%x failed to initialize.\n", sp->sp_id);
+		spmc_sp_synchronous_exit(&sp->ec[idx], x2);
+		/* Should not get here. */
+		panic();
+	}
+
+	return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+}
+
+/*******************************************************************************
+ * This function will parse the Secure Partition Manifest. From manifest, it
+ * will fetch details for preparing Secure partition image context and secure
+ * partition image boot arguments if any.
+ ******************************************************************************/
+static int sp_manifest_parse(void *sp_manifest, int offset,
+			     struct secure_partition_desc *sp,
+			     entry_point_info_t *ep_info)
+{
+	int32_t ret, node;
+	uint32_t config_32;
+
+	/*
+	 * Look for the mandatory fields that are expected to be present in
+	 * the SP manifests.
+	 */
+	node = fdt_path_offset(sp_manifest, "/");
+	if (node < 0) {
+		ERROR("Did not find root node.\n");
+		return node;
+	}
+
+	ret = fdt_read_uint32(sp_manifest, node, "exception-level", &config_32);
+	if (ret != 0) {
+		ERROR("Missing SP Exception Level information.\n");
+		return ret;
+	}
+
+	sp->runtime_el = config_32;
+
+	ret = fdt_read_uint32(sp_manifest, node, "ffa-version", &config_32);
+	if (ret != 0) {
+		ERROR("Missing Secure Partition FF-A Version.\n");
+		return ret;
+	}
+
+	sp->ffa_version = config_32;
+
+	ret = fdt_read_uint32(sp_manifest, node, "execution-state", &config_32);
+	if (ret != 0) {
+		ERROR("Missing Secure Partition Execution State.\n");
+		return ret;
+	}
+
+	sp->execution_state = config_32;
+
+	/*
+	 * Look for the optional fields that are expected to be present in
+	 * an SP manifest.
+	 */
+	ret = fdt_read_uint32(sp_manifest, node, "id", &config_32);
+	if (ret != 0) {
+		WARN("Missing Secure Partition ID.\n");
+	} else {
+		if (!is_ffa_secure_id_valid(config_32)) {
+			ERROR("Invalid Secure Partition ID (0x%x).\n",
+			      config_32);
+			return -EINVAL;
+		}
+		sp->sp_id = config_32;
+	}
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function gets the Secure Partition Manifest base and maps the manifest
+ * region.
+ * Currently only one Secure Partition manifest is considered which is used to
+ * prepare the context for the single Secure Partition.
+ ******************************************************************************/
+static int find_and_prepare_sp_context(void)
+{
+	void *sp_manifest;
+	uintptr_t manifest_base;
+	uintptr_t manifest_base_align;
+	entry_point_info_t *next_image_ep_info;
+	int32_t ret;
+	struct secure_partition_desc *sp;
+
+	next_image_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
+	if (next_image_ep_info == NULL) {
+		WARN("No Secure Partition image provided by BL2.\n");
+		return -ENOENT;
+	}
+
+	sp_manifest = (void *)next_image_ep_info->args.arg0;
+	if (sp_manifest == NULL) {
+		WARN("Secure Partition manifest absent.\n");
+		return -ENOENT;
+	}
+
+	manifest_base = (uintptr_t)sp_manifest;
+	manifest_base_align = page_align(manifest_base, DOWN);
+
+	/*
+	 * Map the secure partition manifest region in the EL3 translation
+	 * regime.
+	 * Map an area equal to (2 * PAGE_SIZE) for now. During manifest base
+	 * alignment the region of 1 PAGE_SIZE from manifest align base may
+	 * not completely accommodate the secure partition manifest region.
+	 */
+	ret = mmap_add_dynamic_region((unsigned long long)manifest_base_align,
+				      manifest_base_align,
+				      PAGE_SIZE * 2,
+				      MT_RO_DATA);
+	if (ret != 0) {
+		ERROR("Error while mapping SP manifest (%d).\n", ret);
+		return ret;
+	}
+
+	ret = fdt_node_offset_by_compatible(sp_manifest, -1,
+					    "arm,ffa-manifest-1.0");
+	if (ret < 0) {
+		ERROR("Error happened in SP manifest reading.\n");
+		return -EINVAL;
+	}
+
+	/*
+	 * Store the size of the manifest so that it can be used later to pass
+	 * the manifest as boot information later.
+	 */
+	next_image_ep_info->args.arg1 = fdt_totalsize(sp_manifest);
+	INFO("Manifest size = %lu bytes.\n", next_image_ep_info->args.arg1);
+
+	/*
+	 * Select an SP descriptor for initialising the partition's execution
+	 * context on the primary CPU.
+	 */
+	sp = spmc_get_current_sp_ctx();
+
+	/* Initialize entry point information for the SP */
+	SET_PARAM_HEAD(next_image_ep_info, PARAM_EP, VERSION_1,
+		       SECURE | EP_ST_ENABLE);
+
+	/* Parse the SP manifest. */
+	ret = sp_manifest_parse(sp_manifest, ret, sp, next_image_ep_info);
+	if (ret != 0) {
+		ERROR("Error in Secure Partition manifest parsing.\n");
+		return ret;
+	}
+
+	/* Check that the runtime EL in the manifest was correct. */
+	if (sp->runtime_el != S_EL1) {
+		ERROR("Unexpected runtime EL: %d\n", sp->runtime_el);
+		return -EINVAL;
+	}
+
+	/* Perform any common initialisation. */
+	spmc_sp_common_setup(sp, next_image_ep_info);
+
+	/* Perform any initialisation specific to S-EL1 SPs. */
+	spmc_el1_sp_setup(sp, next_image_ep_info);
+
+	/* Initialize the SP context with the required ep info. */
+	spmc_sp_common_ep_commit(sp, next_image_ep_info);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function takes an SP context pointer and performs a synchronous entry
+ * into it.
+ ******************************************************************************/
+uint64_t spmc_sp_synchronous_entry(struct sp_exec_ctx *ec)
+{
+	uint64_t rc;
+
+	assert(ec != NULL);
+
+	/* Assign the context of the SP to this CPU */
+	cm_set_context(&(ec->cpu_ctx), SECURE);
+
+	/* Restore the context assigned above */
+	cm_el1_sysregs_context_restore(SECURE);
+	cm_set_next_eret_context(SECURE);
+
+	/* Invalidate TLBs at EL1. */
+	tlbivmalle1();
+	dsbish();
+
+	/* Enter Secure Partition */
+	rc = spm_secure_partition_enter(&ec->c_rt_ctx);
+
+	/* Save secure state */
+	cm_el1_sysregs_context_save(SECURE);
+
+	return rc;
+}
+
+/*******************************************************************************
+ * SPMC Helper Functions.
+ ******************************************************************************/
+static int32_t sp_init(void)
+{
+	uint64_t rc;
+	struct secure_partition_desc *sp;
+	struct sp_exec_ctx *ec;
+
+	sp = spmc_get_current_sp_ctx();
+	ec = spmc_get_sp_ec(sp);
+	ec->rt_model = RT_MODEL_INIT;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	INFO("Secure Partition (0x%x) init start.\n", sp->sp_id);
+
+	rc = spmc_sp_synchronous_entry(ec);
+	if (rc != 0) {
+		/* Indicate SP init was not successful. */
+		ERROR("SP (0x%x) failed to initialize (%lu).\n",
+		      sp->sp_id, rc);
+		return 0;
+	}
+
+	ec->rt_state = RT_STATE_WAITING;
+	INFO("Secure Partition initialized.\n");
+
+	return 1;
+}
+
+static void initalize_sp_descs(void)
+{
+	struct secure_partition_desc *sp;
+
+	for (unsigned int i = 0U; i < SECURE_PARTITION_COUNT; i++) {
+		sp = &sp_desc[i];
+		sp->sp_id = INV_SP_ID;
+		sp->secondary_ep = 0;
+	}
+}
+
+static void initalize_ns_ep_descs(void)
+{
+	struct ns_endpoint_desc *ns_ep;
+
+	for (unsigned int i = 0U; i < NS_PARTITION_COUNT; i++) {
+		ns_ep = &ns_ep_desc[i];
+		/*
+		 * Clashes with the Hypervisor ID but will not be a
+		 * problem in practice.
+		 */
+		ns_ep->ns_ep_id = 0;
+		ns_ep->ffa_version = 0;
+	}
+}
+
+/*******************************************************************************
+ * Initialize SPMC attributes for the SPMD.
+ ******************************************************************************/
+void spmc_populate_attrs(spmc_manifest_attribute_t *spmc_attrs)
+{
+	spmc_attrs->major_version = FFA_VERSION_MAJOR;
+	spmc_attrs->minor_version = FFA_VERSION_MINOR;
+	spmc_attrs->exec_state = MODE_RW_64;
+	spmc_attrs->spmc_id = FFA_SPMC_ID;
+}
+
+/*******************************************************************************
+ * Initialize contexts of all Secure Partitions.
+ ******************************************************************************/
+int32_t spmc_setup(void)
+{
+	int32_t ret;
+
+	/* Initialize endpoint descriptors */
+	initalize_sp_descs();
+	initalize_ns_ep_descs();
+
+	/* Perform physical SP setup. */
+
+	/* Disable MMU at EL1 (initialized by BL2) */
+	disable_mmu_icache_el1();
+
+	/* Initialize context of the SP */
+	INFO("Secure Partition context setup start.\n");
+
+	ret = find_and_prepare_sp_context();
+	if (ret != 0) {
+		ERROR("Error in SP finding and context preparation.\n");
+		return ret;
+	}
+
+	/* Register init function for deferred init.  */
+	bl31_register_bl32_init(&sp_init);
+
+	INFO("Secure Partition setup done.\n");
+
+	return 0;
+}
+
+/*******************************************************************************
+ * Secure Partition Manager SMC handler.
+ ******************************************************************************/
+uint64_t spmc_smc_handler(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint64_t x1,
+			  uint64_t x2,
+			  uint64_t x3,
+			  uint64_t x4,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	switch (smc_fid) {
+
+	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
+	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
+		return direct_req_smc_handler(smc_fid, secure_origin, x1, x2,
+					      x3, x4, cookie, handle, flags);
+
+	case FFA_MSG_SEND_DIRECT_RESP_SMC32:
+	case FFA_MSG_SEND_DIRECT_RESP_SMC64:
+		return direct_resp_smc_handler(smc_fid, secure_origin, x1, x2,
+					       x3, x4, cookie, handle, flags);
+
+	case FFA_MSG_WAIT:
+		return msg_wait_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+
+	case FFA_ERROR:
+		return ffa_error_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+
+	default:
+		WARN("Unsupported FF-A call 0x%08x.\n", smc_fid);
+		break;
+	}
+	return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+}
diff --git a/services/std_svc/spm/el3_spmc/spmc_setup.c b/services/std_svc/spm/el3_spmc/spmc_setup.c
new file mode 100644
index 0000000..7b23c9e
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_setup.c
@@ -0,0 +1,112 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <context.h>
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/utils.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <plat/common/common_def.h>
+#include <plat/common/platform.h>
+#include <services/ffa_svc.h>
+#include "spm_common.h"
+#include "spmc.h"
+
+#include <platform_def.h>
+
+/*
+ * We are assuming that the index of the execution
+ * context used is the linear index of the current physical cpu.
+ */
+unsigned int get_ec_index(struct secure_partition_desc *sp)
+{
+	return plat_my_core_pos();
+}
+
+/* S-EL1 partition specific initialisation. */
+void spmc_el1_sp_setup(struct secure_partition_desc *sp,
+		       entry_point_info_t *ep_info)
+{
+	/* Sanity check input arguments. */
+	assert(sp != NULL);
+	assert(ep_info != NULL);
+
+	/* Initialise the SPSR for S-EL1 SPs. */
+	ep_info->spsr =	SPSR_64(MODE_EL1, MODE_SP_ELX,
+				DISABLE_ALL_EXCEPTIONS);
+
+	/*
+	 * Check whether setup is being performed for the primary or a secondary
+	 * execution context. In the latter case, indicate to the SP that this
+	 * is a warm boot.
+	 * TODO: This check would need to be reworked if the same entry point is
+	 * used for both primary and secondary initialisation.
+	 */
+	if (sp->secondary_ep != 0U) {
+		/*
+		 * Sanity check that the secondary entry point is still what was
+		 * originally set.
+		 */
+		assert(sp->secondary_ep == ep_info->pc);
+		ep_info->args.arg0 = FFA_WB_TYPE_S2RAM;
+	}
+}
+
+/* Common initialisation for all SPs. */
+void spmc_sp_common_setup(struct secure_partition_desc *sp,
+			  entry_point_info_t *ep_info)
+{
+	uint16_t sp_id;
+
+	/* Assign FF-A Partition ID if not already assigned. */
+	if (sp->sp_id == INV_SP_ID) {
+		sp_id = FFA_SP_ID_BASE + ACTIVE_SP_DESC_INDEX;
+		/*
+		 * Ensure we don't clash with previously assigned partition
+		 * IDs.
+		 */
+		while (!is_ffa_secure_id_valid(sp_id)) {
+			sp_id++;
+
+			if (sp_id == FFA_SWD_ID_LIMIT) {
+				ERROR("Unable to determine valid SP ID.\n");
+				panic();
+			}
+		}
+		sp->sp_id = sp_id;
+	}
+
+	/*
+	 * We currently only support S-EL1 partitions so ensure this is the
+	 * case.
+	 */
+	assert(sp->runtime_el == S_EL1);
+
+	/*
+	 * Clear the general purpose registers. These should be populated as
+	 * required.
+	 */
+	zeromem(&ep_info->args, sizeof(ep_info->args));
+}
+
+/*
+ * Initialise the SP context now we have populated the common and EL specific
+ * entrypoint information.
+ */
+void spmc_sp_common_ep_commit(struct secure_partition_desc *sp,
+			      entry_point_info_t *ep_info)
+{
+	cpu_context_t *cpu_ctx;
+
+	cpu_ctx = &(spmc_get_sp_ec(sp)->cpu_ctx);
+	print_entry_point_info(ep_info);
+	cm_setup_context(cpu_ctx, ep_info);
+}
diff --git a/services/std_svc/spm_mm/aarch64/spm_mm_shim_exceptions.S b/services/std_svc/spm/spm_mm/aarch64/spm_mm_shim_exceptions.S
similarity index 97%
rename from services/std_svc/spm_mm/aarch64/spm_mm_shim_exceptions.S
rename to services/std_svc/spm/spm_mm/aarch64/spm_mm_shim_exceptions.S
index be4084c..836f75c 100644
--- a/services/std_svc/spm_mm/aarch64/spm_mm_shim_exceptions.S
+++ b/services/std_svc/spm/spm_mm/aarch64/spm_mm_shim_exceptions.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/services/std_svc/spm_mm/spm_mm.mk b/services/std_svc/spm/spm_mm/spm_mm.mk
similarity index 78%
rename from services/std_svc/spm_mm/spm_mm.mk
rename to services/std_svc/spm/spm_mm/spm_mm.mk
index a87bdd8..78ef0c9 100644
--- a/services/std_svc/spm_mm/spm_mm.mk
+++ b/services/std_svc/spm/spm_mm/spm_mm.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -17,11 +17,10 @@
         $(error "Error: SPM_MM is not compatible with ENABLE_SME_FOR_NS")
 endif
 
-SPM_SOURCES	:=	$(addprefix services/std_svc/spm_mm/,	\
-			${ARCH}/spm_mm_helpers.S			\
+SPM_MM_SOURCES	:=	$(addprefix services/std_svc/spm/spm_mm/,	\
 			${ARCH}/spm_mm_shim_exceptions.S		\
-			spm_mm_main.c				\
-			spm_mm_setup.c				\
+			spm_mm_main.c					\
+			spm_mm_setup.c					\
 			spm_mm_xlat.c)
 
 
diff --git a/services/std_svc/spm_mm/spm_mm_main.c b/services/std_svc/spm/spm_mm/spm_mm_main.c
similarity index 98%
rename from services/std_svc/spm_mm/spm_mm_main.c
rename to services/std_svc/spm/spm_mm/spm_mm_main.c
index 14c0038..e71e65b 100644
--- a/services/std_svc/spm_mm/spm_mm_main.c
+++ b/services/std_svc/spm/spm_mm/spm_mm_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -22,6 +22,7 @@
 #include <services/spm_mm_svc.h>
 #include <smccc_helpers.h>
 
+#include "spm_common.h"
 #include "spm_mm_private.h"
 
 /*******************************************************************************
diff --git a/services/std_svc/spm_mm/spm_mm_private.h b/services/std_svc/spm/spm_mm/spm_mm_private.h
similarity index 88%
rename from services/std_svc/spm_mm/spm_mm_private.h
rename to services/std_svc/spm/spm_mm/spm_mm_private.h
index 45b4789..0eff1c0 100644
--- a/services/std_svc/spm_mm/spm_mm_private.h
+++ b/services/std_svc/spm/spm_mm/spm_mm_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 #define SPM_MM_PRIVATE_H
 
 #include <context.h>
+#include "spm_common.h"
 
 /*******************************************************************************
  * Constants that allow assembler code to preserve callee-saved registers of the
@@ -51,9 +52,6 @@
 	spinlock_t state_lock;
 } sp_context_t;
 
-/* Assembly helpers */
-uint64_t spm_secure_partition_enter(uint64_t *c_rt_ctx);
-void __dead2 spm_secure_partition_exit(uint64_t c_rt_ctx, uint64_t ret);
 
 void spm_sp_setup(sp_context_t *sp_ctx);
 
diff --git a/services/std_svc/spm_mm/spm_mm_setup.c b/services/std_svc/spm/spm_mm/spm_mm_setup.c
similarity index 98%
rename from services/std_svc/spm_mm/spm_mm_setup.c
rename to services/std_svc/spm/spm_mm/spm_mm_setup.c
index 9d681c2..04dc212 100644
--- a/services/std_svc/spm_mm/spm_mm_setup.c
+++ b/services/std_svc/spm/spm_mm/spm_mm_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  * Copyright (c) 2021, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -19,6 +19,7 @@
 #include <plat/common/platform.h>
 #include <services/spm_mm_partition.h>
 
+#include "spm_common.h"
 #include "spm_mm_private.h"
 #include "spm_mm_shim_private.h"
 
diff --git a/services/std_svc/spm_mm/spm_mm_shim_private.h b/services/std_svc/spm/spm_mm/spm_mm_shim_private.h
similarity index 90%
rename from services/std_svc/spm_mm/spm_mm_shim_private.h
rename to services/std_svc/spm/spm_mm/spm_mm_shim_private.h
index 0c8d894..f69c748 100644
--- a/services/std_svc/spm_mm/spm_mm_shim_private.h
+++ b/services/std_svc/spm/spm_mm/spm_mm_shim_private.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/services/std_svc/spm_mm/spm_mm_xlat.c b/services/std_svc/spm/spm_mm/spm_mm_xlat.c
similarity index 98%
rename from services/std_svc/spm_mm/spm_mm_xlat.c
rename to services/std_svc/spm/spm_mm/spm_mm_xlat.c
index eae597c..6261016 100644
--- a/services/std_svc/spm_mm/spm_mm_xlat.c
+++ b/services/std_svc/spm/spm_mm/spm_mm_xlat.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 27a8382..5b131cd 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -24,6 +24,7 @@
 #include <plat/common/platform.h>
 #include <platform_def.h>
 #include <services/ffa_svc.h>
+#include <services/spmc_svc.h>
 #include <services/spmd_svc.h>
 #include <smccc_helpers.h>
 #include "spmd_private.h"
@@ -34,7 +35,8 @@
 static spmd_spm_core_context_t spm_core_context[PLATFORM_CORE_COUNT];
 
 /*******************************************************************************
- * SPM Core attribute information read from its manifest.
+ * SPM Core attribute information is read from its manifest if the SPMC is not
+ * at EL3. Else, it is populated from the SPMC directly.
  ******************************************************************************/
 static spmc_manifest_attribute_t spmc_attrs;
 
@@ -88,7 +90,9 @@
 				 uint64_t x2,
 				 uint64_t x3,
 				 uint64_t x4,
-				 void *handle);
+				 void *cookie,
+				 void *handle,
+				 uint64_t flags);
 
 /******************************************************************************
  * Builds an SPMD to SPMC direct message request.
@@ -385,8 +389,23 @@
  ******************************************************************************/
 int spmd_setup(void)
 {
-	void *spmc_manifest;
 	int rc;
+	void *spmc_manifest;
+
+	/*
+	 * If the SPMC is at EL3, then just initialise it directly. The
+	 * shenanigans of when it is at a lower EL are not needed.
+	 */
+	if (is_spmc_at_el3()) {
+		/* Allow the SPMC to populate its attributes directly. */
+		spmc_populate_attrs(&spmc_attrs);
+
+		rc = spmc_setup();
+		if (rc != 0) {
+			ERROR("SPMC initialisation failed 0x%x.\n", rc);
+		}
+		return rc;
+	}
 
 	spmc_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
 	if (spmc_ep_info == NULL) {
@@ -417,15 +436,15 @@
 }
 
 /*******************************************************************************
- * Forward SMC to the other security state
+ * Forward FF-A SMCs 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)
+uint64_t spmd_smc_switch_state(uint32_t smc_fid,
+			       bool secure_origin,
+			       uint64_t x1,
+			       uint64_t x2,
+			       uint64_t x3,
+			       uint64_t x4,
+			       void *handle)
 {
 	unsigned int secure_state_in = (secure_origin) ? SECURE : NON_SECURE;
 	unsigned int secure_state_out = (!secure_origin) ? SECURE : NON_SECURE;
@@ -458,6 +477,28 @@
 }
 
 /*******************************************************************************
+ * Forward SMCs 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 *cookie,
+				 void *handle,
+				 uint64_t flags)
+{
+	if (is_spmc_at_el3() && !secure_origin) {
+		return spmc_smc_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+	}
+	return spmd_smc_switch_state(smc_fid, secure_origin, x1, x2, x3, x4,
+				     handle);
+
+}
+
+/*******************************************************************************
  * Return FFA_ERROR with specified error code
  ******************************************************************************/
 static uint64_t spmd_ffa_error_return(void *handle, int error_code)
@@ -484,6 +525,10 @@
  *****************************************************************************/
 static bool spmd_is_spmc_message(unsigned int ep)
 {
+	if (is_spmc_at_el3()) {
+		return false;
+	}
+
 	return ((ffa_endpoint_destination(ep) == SPMD_DIRECT_MSG_ENDPOINT_ID)
 		&& (ffa_endpoint_source(ep) == spmc_attrs.spmc_id));
 }
@@ -502,6 +547,35 @@
 }
 
 /*******************************************************************************
+ * This function forwards FF-A SMCs to either the main SPMD handler or the
+ * SPMC at EL3, depending on the origin security state, if enabled.
+ ******************************************************************************/
+uint64_t spmd_ffa_smc_handler(uint32_t smc_fid,
+			      uint64_t x1,
+			      uint64_t x2,
+			      uint64_t x3,
+			      uint64_t x4,
+			      void *cookie,
+			      void *handle,
+			      uint64_t flags)
+{
+	if (is_spmc_at_el3()) {
+		/*
+		 * If we have an SPMC at EL3 allow handling of the SMC first.
+		 * The SPMC will call back through to SPMD handler if required.
+		 */
+		if (is_caller_secure(flags)) {
+			return spmc_smc_handler(smc_fid,
+						is_caller_secure(flags),
+						x1, x2, x3, x4, cookie,
+						handle, flags);
+		}
+	}
+	return spmd_smc_handler(smc_fid, x1, x2, x3, x4, cookie,
+				handle, flags);
+}
+
+/*******************************************************************************
  * This function handles all SMCs in the range reserved for FFA. Each call is
  * either forwarded to the other security state or handled by the SPM dispatcher
  ******************************************************************************/
@@ -542,7 +616,8 @@
 		}
 
 		return spmd_smc_forward(smc_fid, secure_origin,
-					x1, x2, x3, x4, handle);
+					x1, x2, x3, x4, cookie,
+					handle, flags);
 		break; /* not reached */
 
 	case FFA_VERSION:
@@ -553,9 +628,11 @@
 		 * If caller is non secure and SPMC was initialized,
 		 * return SPMC's version.
 		 * Sanity check to "input_version".
+		 * If the EL3 SPMC is enabled, ignore the SPMC state as
+		 * this is not used.
 		 */
 		if ((input_version & FFA_VERSION_BIT31_MASK) ||
-			(ctx->state == SPMC_STATE_RESET)) {
+		    (!is_spmc_at_el3() && (ctx->state == SPMC_STATE_RESET))) {
 			ret = FFA_ERROR_NOT_SUPPORTED;
 		} else if (!secure_origin) {
 			gp_regs_t *gpregs = get_gpregs_ctx(&ctx->cpu_ctx);
@@ -610,7 +687,8 @@
 			 */
 			return spmd_smc_forward(ret, true, FFA_PARAM_MBZ,
 						FFA_PARAM_MBZ, FFA_PARAM_MBZ,
-						FFA_PARAM_MBZ, gpregs);
+						FFA_PARAM_MBZ, cookie, gpregs,
+						flags);
 		} else {
 			ret = MAKE_FFA_VERSION(FFA_VERSION_MAJOR,
 					       FFA_VERSION_MINOR);
@@ -630,7 +708,8 @@
 		/* Forward SMC from Normal world to the SPM Core */
 		if (!secure_origin) {
 			return spmd_smc_forward(smc_fid, secure_origin,
-						x1, x2, x3, x4, handle);
+						x1, x2, x3, x4, cookie,
+						handle, flags);
 		}
 
 		/*
@@ -726,7 +805,8 @@
 		} else {
 			/* Forward direct message to the other world */
 			return spmd_smc_forward(smc_fid, secure_origin,
-				x1, x2, x3, x4, handle);
+						x1, x2, x3, x4, cookie,
+						handle, flags);
 		}
 		break; /* Not reached */
 
@@ -736,7 +816,8 @@
 		} else {
 			/* Forward direct message to the other world */
 			return spmd_smc_forward(smc_fid, secure_origin,
-				x1, x2, x3, x4, handle);
+						x1, x2, x3, x4, cookie,
+						handle, flags);
 		}
 		break; /* Not reached */
 
@@ -755,6 +836,7 @@
 	case FFA_NOTIFICATION_INFO_GET:
 	case FFA_NOTIFICATION_INFO_GET_SMC64:
 	case FFA_MSG_SEND2:
+	case FFA_RX_ACQUIRE:
 #endif
 	case FFA_MSG_RUN:
 		/*
@@ -791,7 +873,8 @@
 		 */
 
 		return spmd_smc_forward(smc_fid, secure_origin,
-					x1, x2, x3, x4, handle);
+					x1, x2, x3, x4, cookie,
+					handle, flags);
 		break; /* not reached */
 
 	case FFA_MSG_WAIT:
@@ -814,7 +897,8 @@
 		}
 
 		return spmd_smc_forward(smc_fid, secure_origin,
-					x1, x2, x3, x4, handle);
+					x1, x2, x3, x4, cookie,
+					handle, flags);
 		break; /* not reached */
 
 	case FFA_NORMAL_WORLD_RESUME:
diff --git a/services/std_svc/spmd/spmd_private.h b/services/std_svc/spmd/spmd_private.h
index 4cd6a74..4c298c9 100644
--- a/services/std_svc/spmd/spmd_private.h
+++ b/services/std_svc/spmd/spmd_private.h
@@ -58,12 +58,6 @@
  */
 #define FFA_NS_ENDPOINT_ID			U(0)
 
-/* Mask and shift to check valid secure FF-A Endpoint ID. */
-#define SPMC_SECURE_ID_MASK			U(1)
-#define SPMC_SECURE_ID_SHIFT			U(15)
-
-#define SPMD_DIRECT_MSG_ENDPOINT_ID		U(FFA_ENDPOINT_ID_MAX - 1)
-
 /* Define SPMD target function IDs for framework messages to the SPMC */
 #define SPMD_FWK_MSG_BIT			BIT(31)
 #define SPMD_FWK_MSG_PSCI			U(0)
diff --git a/services/std_svc/std_svc_setup.c b/services/std_svc/std_svc_setup.c
index bfe26ca..b1e3db9 100644
--- a/services/std_svc/std_svc_setup.c
+++ b/services/std_svc/std_svc_setup.c
@@ -17,6 +17,7 @@
 #include <services/rmmd_svc.h>
 #include <services/sdei.h>
 #include <services/spm_mm_svc.h>
+#include <services/spmc_svc.h>
 #include <services/spmd_svc.h>
 #include <services/std_svc.h>
 #include <services/trng_svc.h>
@@ -147,8 +148,8 @@
 	 * dispatcher and return its return value
 	 */
 	if (is_ffa_fid(smc_fid)) {
-		return spmd_smc_handler(smc_fid, x1, x2, x3, x4, cookie,
-					handle, flags);
+		return spmd_ffa_smc_handler(smc_fid, x1, x2, x3, x4, cookie,
+					    handle, flags);
 	}
 #endif