diff --git a/Makefile b/Makefile
index b42bdc5..05d97b0 100644
--- a/Makefile
+++ b/Makefile
@@ -833,6 +833,10 @@
     endif
 endif
 
+ifeq ($(DRTM_SUPPORT),1)
+    $(info DRTM_SUPPORT is an experimental feature)
+endif
+
 ################################################################################
 # Process platform overrideable behaviour
 ################################################################################
@@ -1008,6 +1012,7 @@
         HW_ASSISTED_COHERENCY \
         INVERTED_MEMMAP \
         MEASURED_BOOT \
+        DRTM_SUPPORT \
         NS_TIMER_SWITCH \
         OVERRIDE_LIBC \
         PL011_GENERIC_UART \
@@ -1144,6 +1149,7 @@
         HW_ASSISTED_COHERENCY \
         LOG_LEVEL \
         MEASURED_BOOT \
+        DRTM_SUPPORT \
         NS_TIMER_SWITCH \
         PL011_GENERIC_UART \
         PLAT_${PLAT} \
diff --git a/bl1/aarch32/bl1_exceptions.S b/bl1/aarch32/bl1_exceptions.S
index 493d2ca..4a6815f 100644
--- a/bl1/aarch32/bl1_exceptions.S
+++ b/bl1/aarch32/bl1_exceptions.S
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -118,6 +118,14 @@
 	mov	r0, #DISABLE_DCACHE
 	bl	enable_mmu_svc_mon
 
+	/*
+	 * Invalidate `smc_ctx_t` in data cache to prevent dirty data being
+	 * used.
+	 */
+	mov	r0, r6
+	mov	r1, #SMC_CTX_SIZE
+	bl	inv_dcache_range
+
 	/* Enable the data cache. */
 	ldcopr	r9, SCTLR
 	orr	r9, r9, #SCTLR_C_BIT
diff --git a/changelog.yaml b/changelog.yaml
index cc81371..e2184e4 100644
--- a/changelog.yaml
+++ b/changelog.yaml
@@ -645,6 +645,9 @@
                   - title: GIC-600AE
                     scope: gic600ae
 
+          - title: SMMU
+            scope: smmu
+
           - title: TZC
             scope: tzc
 
diff --git a/common/uuid.c b/common/uuid.c
index ac6db50..3e47eb4 100644
--- a/common/uuid.c
+++ b/common/uuid.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -132,3 +132,27 @@
 	return 0;
 }
 
+/*
+ * Helper function to check if 2 UUIDs match.
+ */
+bool uuid_match(uint32_t *uuid1, uint32_t *uuid2)
+{
+	return !memcmp(uuid1, uuid2, sizeof(uint32_t) * 4);
+}
+
+/*
+ * Helper function to copy from one UUID struct to another.
+ */
+void copy_uuid(uint32_t *to_uuid, uint32_t *from_uuid)
+{
+	to_uuid[0] = from_uuid[0];
+	to_uuid[1] = from_uuid[1];
+	to_uuid[2] = from_uuid[2];
+	to_uuid[3] = from_uuid[3];
+}
+
+bool is_null_uuid(uint32_t *uuid)
+{
+	return (uuid[0] == 0 && uuid[1] == 0 &&
+		uuid[2] == 0 && uuid[3] == 0);
+}
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index c62a6be..159a3db 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -75,8 +75,6 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Mark Dykes <mark.dykes@arm.com>
 :|G|: `mardyk01`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: services/std_svc/sdei/
 
 Trusted Boot
@@ -89,8 +87,14 @@
 :|G|: `ManishVB-Arm`_
 :|F|: drivers/auth/
 
-Secure Partition Manager (SPM)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+Secure Partition Manager Core (EL3 FF-A SPMC)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+:|M|: Marc Bonnici <marc.bonnici@arm.com>
+:|G|: `marcbonnici`_
+:|F|: services/std_svc/spm/el3_spmc/\*
+
+Secure Partition Manager Dispatcher (SPMD)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Olivier Deprez <olivier.deprez@arm.com>
 :|G|: `odeprez`_
 :|M|: Manish Pandey <manish.pandey2@arm.com>
@@ -99,14 +103,12 @@
 :|G|: `max-shvetsov`_
 :|M|: Joao Alves <Joao.Alves@arm.com>
 :|G|: `J-Alves`_
-:|F|: services/std_svc/spm\*
+:|F|: services/std_svc/spmd/\*
 
 Exception Handling Framework (EHF)
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Manish Badarkhe <manish.badarkhe@arm.com>
 :|G|: `ManishVB-Arm`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: bl31/ehf.c
 
 Realm Management Extension (RME)
@@ -115,8 +117,6 @@
 :|G|: `bipinravi-arm`_
 :|M|: Mark Dykes <mark.dykes@arm.com>
 :|G|: `mardyk01`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|M|: Zelalem Aweke <Zelalem.Aweke@arm.com>
 :|G|: `zelalem-aweke`_
 
@@ -193,16 +193,12 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Joao Alves <Joao.Alves@arm.com>
 :|G|: `J-Alves`_
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
 :|F|: lib/pmf/
 
 Arm CPU libraries
 ^^^^^^^^^^^^^^^^^
 :|M|: Lauren Wehrmeister <Lauren.Wehrmeister@arm.com>
 :|G|: `laurenw-arm`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: lib/cpus/
 
 Reliability Availability Serviceabilty (RAS) framework
@@ -225,8 +221,6 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Zelalem Aweke <Zelalem.Aweke@arm.com>
 :|G|: `zelalem-aweke`_
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
 :|F|: lib/extensions/mpam/
 
 Pointer Authentication (PAuth) and Branch Target Identification (BTI) extensions
@@ -241,22 +235,12 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Zelalem Aweke <Zelalem.Aweke@arm.com>
 :|G|: `zelalem-aweke`_
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
 :|F|: lib/extensions/spe/
 
-Scalable Vector Extension (SVE)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
-:|M|: Jimmy Brisson <Jimmy.Brisson@arm.com>
-:|G|: `theotherjimmy`_
-:|F|: lib/extensions/sve/
-
 Standard C library
 ^^^^^^^^^^^^^^^^^^
 :|M|: Alexei Fedorov <Alexei.Fedorov@arm.com>
 :|G|: `AlexeiFedorov`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: lib/libc/
 
 Library At ROM (ROMlib)
@@ -372,8 +356,6 @@
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 :|M|: Mark Dykes <mark.dykes@arm.com>
 :|G|: `mardyk01`_
-:|M|: John Powell <john.powell@arm.com>
-:|G|: `john-powell-arm`_
 :|F|: lib/gpt_rme
 :|F|: include/lib/gpt_rme
 
@@ -890,10 +872,8 @@
 .. _javieralso-arm: https://github.com/javieralso-arm
 .. _laurenw-arm: https://github.com/laurenw-arm
 .. _zelalem-aweke: https://github.com/zelalem-aweke
-.. _theotherjimmy: https://github.com/theotherjimmy
 .. _J-Alves: https://github.com/J-Alves
 .. _madhukar-Arm: https://github.com/madhukar-Arm
-.. _john-powell-arm: https://github.com/john-powell-arm
 .. _raghuncstate: https://github.com/raghuncstate
 .. _CJKay: https://github.com/cjkay
 .. _nmenon: https://github.com/nmenon
@@ -908,5 +888,6 @@
 .. _JiafeiPan: https://github.com/JiafeiPan
 .. _arve-android: https://github.com/arve-android
 .. _marcone: https://github.com/marcone
+.. _marcbonnici: https://github.com/marcbonnici
 
 .. _Project Maintenance Process: https://developer.trustedfirmware.org/w/collaboration/project-maintenance-process/
diff --git a/docs/components/ffa-manifest-binding.rst b/docs/components/ffa-manifest-binding.rst
index df2985c..59996cc 100644
--- a/docs/components/ffa-manifest-binding.rst
+++ b/docs/components/ffa-manifest-binding.rst
@@ -151,6 +151,16 @@
    - List of <u32> tuples, identifying the IDs this partition is acting as
      proxy for.
 
+- power-management-messages
+   - value type: <u32>
+   - Specifies which power management messages a partition subscribes to.
+     A set bit means the partition should be informed of the power event, clear
+     bit - should not be informed of event:
+
+      - Bit[0]: CPU_OFF
+      - Bit[1]: CPU_SUSPEND
+      - Bit[2]: CPU_SUSPEND_RESUME
+
 Memory Regions
 --------------
 
diff --git a/docs/getting_started/build-options.rst b/docs/getting_started/build-options.rst
index 742b6b5..be50e5e 100644
--- a/docs/getting_started/build-options.rst
+++ b/docs/getting_started/build-options.rst
@@ -649,6 +649,15 @@
 
    This option defaults to 0.
 
+-  ``DRTM_SUPPORT``: Boolean flag to enable support for Dynamic Root of Trust
+   for Measurement (DRTM). This feature has trust dependency on BL31 for taking
+   the measurements and recording them as per `PSA DRTM specification`_. For
+   platforms which use BL2 to load/authenticate BL31 ``TRUSTED_BOARD_BOOT`` can
+   be used and for the platforms which use ``RESET_TO_BL31`` platform owners
+   should have mechanism to authenticate BL31.
+
+   This option defaults to 0.
+
 -  ``NON_TRUSTED_WORLD_KEY``: This option is used when ``GENERATE_COT=1``. It
    specifies the file that contains the Non-Trusted World private key in PEM
    format. If ``SAVE_KEYS=1``, this file name will be used to save the key.
@@ -1116,3 +1125,4 @@
 
 .. _DEN0115: https://developer.arm.com/docs/den0115/latest
 .. _PSA FW update specification: https://developer.arm.com/documentation/den0118/a/
+.. _PSA DRTM specification: https://developer.arm.com/documentation/den0113/a
diff --git a/docs/getting_started/prerequisites.rst b/docs/getting_started/prerequisites.rst
index 3a54e69..c625090 100644
--- a/docs/getting_started/prerequisites.rst
+++ b/docs/getting_started/prerequisites.rst
@@ -54,7 +54,7 @@
 The following libraries must be available to build one or more components or
 supporting tools:
 
-- OpenSSL >= 1.0.1
+- OpenSSL >= 3.0
 
    Required to build the cert_create tool.
 
diff --git a/docs/plat/arm/fvp/index.rst b/docs/plat/arm/fvp/index.rst
index 9280f7b..3d10e45 100644
--- a/docs/plat/arm/fvp/index.rst
+++ b/docs/plat/arm/fvp/index.rst
@@ -12,7 +12,7 @@
 (64-bit host machine only).
 
 .. note::
-   The FVP models used are Version 11.16 Build 16, unless otherwise stated.
+   The FVP models used are Version 11.17 Build 21, unless otherwise stated.
 
 -  ``Foundation_Platform``
 -  ``FVP_Base_AEMv8A-AEMv8A-AEMv8A-AEMv8A-CCN502``
@@ -48,12 +48,12 @@
 -  ``FVP_Base_Neoverse-N2x4`` (Version 11.12 build 38)
 -  ``FVP_Base_Neoverse-V1x4``
 -  ``FVP_Base_RevC-2xAEMvA``  (For certain configurations also uses 0.0/6557)
--  ``FVP_CSS_SGI-575``        (Version 11.15/26)
--  ``FVP_Morello``            (Version 0.11/19)
--  ``FVP_RD_E1_edge``         (Version 11.15/26)
--  ``FVP_RD_N1_edge_dual``    (Version 11.15/26)
--  ``FVP_RD_N1_edge``         (Version 11.15/26)
--  ``FVP_RD_V1``              (Version 11.15/26)
+-  ``FVP_CSS_SGI-575``        (Version 11.17/33)
+-  ``FVP_Morello``            (Version 0.11/33)
+-  ``FVP_RD_E1_edge``         (Version 11.17/33)
+-  ``FVP_RD_N1_edge_dual``    (Version 11.17/33)
+-  ``FVP_RD_N1_edge``         (Version 11.17/33)
+-  ``FVP_RD_V1``              (Version 11.17/33)
 -  ``FVP_TC0``
 -  ``FVP_TC1``
 
diff --git a/drivers/arm/smmu/smmu_v3.c b/drivers/arm/smmu/smmu_v3.c
index 45f6df9..6c6f978 100644
--- a/drivers/arm/smmu/smmu_v3.c
+++ b/drivers/arm/smmu/smmu_v3.c
@@ -14,7 +14,7 @@
 /* SMMU poll number of retries */
 #define SMMU_POLL_TIMEOUT_US	U(1000)
 
-static int __init smmuv3_poll(uintptr_t smmu_reg, uint32_t mask,
+static int smmuv3_poll(uintptr_t smmu_reg, uint32_t mask,
 				uint32_t value)
 {
 	uint32_t reg_val;
@@ -155,3 +155,28 @@
 	return smmuv3_poll(smmu_base + SMMU_S_INIT,
 				SMMU_S_INIT_INV_ALL, 0U);
 }
+
+int smmuv3_ns_set_abort_all(uintptr_t smmu_base)
+{
+	/* Attribute update has completed when SMMU_GBPA.Update bit is 0 */
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U) {
+		return -1;
+	}
+
+	/*
+	 * Set GBPA's ABORT bit. Other GBPA fields are presumably ignored then,
+	 * so simply preserve their value.
+	 */
+	mmio_setbits_32(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE | SMMU_GBPA_ABORT);
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U) {
+		return -1;
+	}
+
+	/* Disable the SMMU to engage the GBPA fields previously configured. */
+	mmio_clrbits_32(smmu_base + SMMU_CR0, SMMU_CR0_SMMUEN);
+	if (smmuv3_poll(smmu_base + SMMU_CR0ACK, SMMU_CR0_SMMUEN, 0U) != 0U) {
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/drivers/partition/partition.c b/drivers/partition/partition.c
index 7706f88..c84816f 100644
--- a/drivers/partition/partition.c
+++ b/drivers/partition/partition.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,6 +10,7 @@
 #include <string.h>
 
 #include <common/debug.h>
+#include <common/tf_crc32.h>
 #include <drivers/io/io_storage.h>
 #include <drivers/partition/efi.h>
 #include <drivers/partition/partition.h>
@@ -76,7 +77,7 @@
 }
 
 /*
- * Load GPT header and check the GPT signature.
+ * Load GPT header and check the GPT signature and header CRC.
  * If partition numbers could be found, check & update it.
  */
 static int load_gpt_header(uintptr_t image_handle)
@@ -84,6 +85,7 @@
 	gpt_header_t header;
 	size_t bytes_read;
 	int result;
+	uint32_t header_crc, calc_crc;
 
 	result = io_seek(image_handle, IO_SEEK_SET, GPT_HEADER_OFFSET);
 	if (result != 0) {
@@ -99,6 +101,23 @@
 		return -EINVAL;
 	}
 
+	/*
+	 * UEFI Spec 2.8 March 2019 Page 119: HeaderCRC32 value is
+	 * computed by setting this field to 0, and computing the
+	 * 32-bit CRC for HeaderSize bytes.
+	 */
+	header_crc = header.header_crc;
+	header.header_crc = 0U;
+
+	calc_crc = tf_crc32(0U, (uint8_t *)&header, DEFAULT_GPT_HEADER_SIZE);
+	if (header_crc != calc_crc) {
+		ERROR("Invalid GPT Header CRC: Expected 0x%x but got 0x%x.\n",
+		      header_crc, calc_crc);
+		return -EINVAL;
+	}
+
+	header.header_crc = header_crc;
+
 	/* partition numbers can't exceed PLAT_PARTITION_MAX_ENTRIES */
 	list.entry_count = header.list_num;
 	if (list.entry_count > PLAT_PARTITION_MAX_ENTRIES) {
diff --git a/fdts/stm32mp13-fw-config.dtsi b/fdts/stm32mp13-fw-config.dtsi
index dc8ca1b..28f7086 100644
--- a/fdts/stm32mp13-fw-config.dtsi
+++ b/fdts/stm32mp13-fw-config.dtsi
@@ -13,11 +13,9 @@
 #endif
 
 #define DDR_NS_BASE	STM32MP_DDR_BASE
-#define DDR_SEC_SIZE	0x01e00000
+#define DDR_SEC_SIZE	0x02000000
 #define DDR_SEC_BASE	(STM32MP_DDR_BASE + (DDR_SIZE - DDR_SEC_SIZE))
-#define DDR_SHARE_SIZE	0x00200000
-#define DDR_SHARE_BASE	(DDR_SEC_BASE - DDR_SHARE_SIZE)
-#define DDR_NS_SIZE	(DDR_SHARE_BASE - DDR_NS_BASE)
+#define DDR_NS_SIZE	(DDR_SEC_BASE - DDR_NS_BASE)
 
 /dts-v1/;
 
@@ -48,8 +46,6 @@
 		compatible = "st,mem-firewall";
 		memory-ranges = <
 			DDR_NS_BASE DDR_NS_SIZE TZC_REGION_S_NONE TZC_REGION_NSEC_ALL_ACCESS_RDWR
-			DDR_SHARE_BASE DDR_SHARE_SIZE TZC_REGION_S_NONE
-			TZC_REGION_ACCESS_RDWR(STM32MP1_TZC_A7_ID)
 			DDR_SEC_BASE DDR_SEC_SIZE TZC_REGION_S_RDWR 0>;
 	};
 };
diff --git a/include/common/uuid.h b/include/common/uuid.h
index 5651d0d..c8dd681 100644
--- a/include/common/uuid.h
+++ b/include/common/uuid.h
@@ -1,15 +1,18 @@
 /*
- * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2021-2022, Arm Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#ifndef UUID_H
-#define UUID_H
+#ifndef UUID_COMMON_H
+#define UUID_COMMON_H
 
 #define UUID_BYTES_LENGTH	16
 #define UUID_STRING_LENGTH	36
 
 int read_uuid(uint8_t *dest, char *uuid);
+bool uuid_match(uint32_t *uuid1, uint32_t *uuid2);
+void copy_uuid(uint32_t *to_uuid, uint32_t *from_uuid);
+bool is_null_uuid(uint32_t *uuid);
 
-#endif /* UUID_H */
+#endif /* UUID_COMMON_H */
diff --git a/include/drivers/arm/smmu_v3.h b/include/drivers/arm/smmu_v3.h
index e60c754..37da56f 100644
--- a/include/drivers/arm/smmu_v3.h
+++ b/include/drivers/arm/smmu_v3.h
@@ -12,6 +12,8 @@
 #include <platform_def.h>
 
 /* SMMUv3 register offsets from device base */
+#define SMMU_CR0	U(0x0020)
+#define SMMU_CR0ACK	U(0x0024)
 #define SMMU_GBPA	U(0x0044)
 #define SMMU_S_IDR1	U(0x8004)
 #define SMMU_S_INIT	U(0x803c)
@@ -37,6 +39,9 @@
 
 #endif /* ENABLE_RME */
 
+/* SMMU_CR0 and SMMU_CR0ACK register fields */
+#define SMMU_CR0_SMMUEN			(1UL << 0)
+
 /* SMMU_GBPA register fields */
 #define SMMU_GBPA_UPDATE		(1UL << 31)
 #define SMMU_GBPA_ABORT			(1UL << 20)
@@ -61,4 +66,6 @@
 int smmuv3_init(uintptr_t smmu_base);
 int smmuv3_security_init(uintptr_t smmu_base);
 
+int smmuv3_ns_set_abort_all(uintptr_t smmu_base);
+
 #endif /* SMMU_V3_H */
diff --git a/include/drivers/partition/partition.h b/include/drivers/partition/partition.h
index b292ec7..11e5acf 100644
--- a/include/drivers/partition/partition.h
+++ b/include/drivers/partition/partition.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -29,6 +29,8 @@
 
 #define LEGACY_PARTITION_BLOCK_SIZE	512
 
+#define DEFAULT_GPT_HEADER_SIZE 	92
+
 typedef struct partition_entry {
 	uint64_t		start;
 	uint64_t		length;
diff --git a/include/lib/utils_def.h b/include/lib/utils_def.h
index 7a7012d..198b890 100644
--- a/include/lib/utils_def.h
+++ b/include/lib/utils_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
  * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
@@ -104,6 +104,13 @@
 #define round_down(value, boundary)		\
 	((value) & ~round_boundary(value, boundary))
 
+/**
+ * Helper macro to ensure a value lies on a given boundary.
+ */
+#define is_aligned(value, boundary)			\
+	(round_up((uintptr_t) value, boundary) ==	\
+	 round_down((uintptr_t) value, boundary))
+
 /*
  * Evaluates to 1 if (ptr + inc) overflows, 0 otherwise.
  * Both arguments must be unsigned pointer values (i.e. uintptr_t).
diff --git a/include/plat/arm/common/arm_reclaim_init.ld.S b/include/plat/arm/common/arm_reclaim_init.ld.S
index 717f65e..788e9ff 100644
--- a/include/plat/arm/common/arm_reclaim_init.ld.S
+++ b/include/plat/arm/common/arm_reclaim_init.ld.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
  */
@@ -12,7 +12,7 @@
             . = . + PLATFORM_STACK_SIZE;
             . = ALIGN(PAGE_SIZE);
             __INIT_CODE_START__ = .;
-	    *(*text.init*);
+	    *(*text.init.*);
             __INIT_CODE_END__ = .;
             INIT_CODE_END_ALIGNED = ALIGN(PAGE_SIZE);
         } >RAM
diff --git a/include/plat/common/platform.h b/include/plat/common/platform.h
index 7664509..b62a631 100644
--- a/include/plat/common/platform.h
+++ b/include/plat/common/platform.h
@@ -347,6 +347,10 @@
 int plat_spm_core_manifest_load(spmc_manifest_attribute_t *manifest,
 				const void *pm_addr);
 #endif
+#if defined(SPMC_AT_EL3)
+int plat_spmc_shmem_datastore_get(uint8_t **datastore, size_t *size);
+#endif
+
 /*******************************************************************************
  * Mandatory BL image load functions(may be overridden).
  ******************************************************************************/
diff --git a/include/services/el3_spmc_ffa_memory.h b/include/services/el3_spmc_ffa_memory.h
new file mode 100644
index 0000000..2037eca
--- /dev/null
+++ b/include/services/el3_spmc_ffa_memory.h
@@ -0,0 +1,258 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef EL3_SPMC_FFA_MEM_H
+#define EL3_SPMC_FFA_MEM_H
+
+#include <assert.h>
+
+/*
+ * Subset of Arm Firmware Framework for Armv8-A
+ * (https://developer.arm.com/docs/den0077/a) needed for shared memory.
+ */
+
+/**
+ * typedef ffa_endpoint_id16_t - Endpoint ID
+ *
+ * Current implementation only supports VM IDs. FF-A spec also support stream
+ * endpoint ids.
+ */
+typedef uint16_t ffa_endpoint_id16_t;
+
+/**
+ * struct ffa_cons_mrd - Constituent memory region descriptor
+ * @address:
+ *         Start address of contiguous memory region. Must be 4K page aligned.
+ * @page_count:
+ *         Number of 4K pages in region.
+ * @reserved_12_15:
+ *         Reserve bytes 12-15 to pad struct size to 16 bytes.
+ */
+struct ffa_cons_mrd {
+	uint64_t address;
+	uint32_t page_count;
+	uint32_t reserved_12_15;
+};
+CASSERT(sizeof(struct ffa_cons_mrd) == 16, assert_ffa_cons_mrd_size_mismatch);
+
+/**
+ * struct ffa_comp_mrd - Composite memory region descriptor
+ * @total_page_count:
+ *         Number of 4k pages in memory region. Must match sum of
+ *         @address_range_array[].page_count.
+ * @address_range_count:
+ *         Number of entries in @address_range_array.
+ * @reserved_8_15:
+ *         Reserve bytes 8-15 to pad struct size to 16 byte alignment and
+ *         make @address_range_array 16 byte aligned.
+ * @address_range_array:
+ *         Array of &struct ffa_cons_mrd entries.
+ */
+struct ffa_comp_mrd {
+	uint32_t total_page_count;
+	uint32_t address_range_count;
+	uint64_t reserved_8_15;
+	struct ffa_cons_mrd address_range_array[];
+};
+CASSERT(sizeof(struct ffa_comp_mrd) == 16, assert_ffa_comp_mrd_size_mismatch);
+
+/**
+ * typedef ffa_mem_attr8_t - Memory region attributes v1.0.
+ * typedef ffa_mem_attr16_t - Memory region attributes v1.1.
+ *
+ * * @FFA_MEM_ATTR_NS_BIT:
+ *     Memory security state.
+ * * @FFA_MEM_ATTR_DEVICE_NGNRNE:
+ *     Device-nGnRnE.
+ * * @FFA_MEM_ATTR_DEVICE_NGNRE:
+ *     Device-nGnRE.
+ * * @FFA_MEM_ATTR_DEVICE_NGRE:
+ *     Device-nGRE.
+ * * @FFA_MEM_ATTR_DEVICE_GRE:
+ *     Device-GRE.
+ * * @FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED
+ *     Normal memory. Non-cacheable.
+ * * @FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB
+ *     Normal memory. Write-back cached.
+ * * @FFA_MEM_ATTR_NON_SHAREABLE
+ *     Non-shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+ * * @FFA_MEM_ATTR_OUTER_SHAREABLE
+ *     Outer Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+ * * @FFA_MEM_ATTR_INNER_SHAREABLE
+ *     Inner Shareable. Combine with FFA_MEM_ATTR_NORMAL_MEMORY_*.
+ */
+typedef uint8_t ffa_mem_attr8_t;
+typedef uint16_t ffa_mem_attr16_t;
+#define FFA_MEM_ATTR_NS_BIT			(0x1U << 6)
+#define FFA_MEM_ATTR_DEVICE_NGNRNE		((1U << 4) | (0x0U << 2))
+#define FFA_MEM_ATTR_DEVICE_NGNRE		((1U << 4) | (0x1U << 2))
+#define FFA_MEM_ATTR_DEVICE_NGRE		((1U << 4) | (0x2U << 2))
+#define FFA_MEM_ATTR_DEVICE_GRE			((1U << 4) | (0x3U << 2))
+#define FFA_MEM_ATTR_NORMAL_MEMORY_UNCACHED	((2U << 4) | (0x1U << 2))
+#define FFA_MEM_ATTR_NORMAL_MEMORY_CACHED_WB	((2U << 4) | (0x3U << 2))
+#define FFA_MEM_ATTR_NON_SHAREABLE		(0x0U << 0)
+#define FFA_MEM_ATTR_OUTER_SHAREABLE		(0x2U << 0)
+#define FFA_MEM_ATTR_INNER_SHAREABLE		(0x3U << 0)
+
+/**
+ * typedef ffa_mem_perm8_t - Memory access permissions
+ *
+ * * @FFA_MEM_ATTR_RO
+ *     Request or specify read-only mapping.
+ * * @FFA_MEM_ATTR_RW
+ *     Request or allow read-write mapping.
+ * * @FFA_MEM_PERM_NX
+ *     Deny executable mapping.
+ * * @FFA_MEM_PERM_X
+ *     Request executable mapping.
+ */
+typedef uint8_t ffa_mem_perm8_t;
+#define FFA_MEM_PERM_RO		(1U << 0)
+#define FFA_MEM_PERM_RW		(1U << 1)
+#define FFA_MEM_PERM_NX		(1U << 2)
+#define FFA_MEM_PERM_X		(1U << 3)
+
+/**
+ * typedef ffa_mem_flag8_t - Endpoint memory flags
+ *
+ * * @FFA_MEM_FLAG_NON_RETRIEVAL_BORROWER
+ *     Non-retrieval Borrower. Memory region must not be or was not retrieved on
+ *     behalf of this endpoint.
+ */
+typedef uint8_t ffa_mem_flag8_t;
+#define FFA_MEM_FLAG_NON_RETRIEVAL_BORROWER	(1U << 0)
+
+/**
+ * typedef ffa_mtd_flag32_t - Memory transaction descriptor flags
+ *
+ * * @FFA_MTD_FLAG_ZERO_MEMORY
+ *     Zero memory after unmapping from sender (must be 0 for share).
+ * * @FFA_MTD_FLAG_TIME_SLICING
+ *     Not supported by this implementation.
+ * * @FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH
+ *     Zero memory after unmapping from borrowers (must be 0 for share).
+ * * @FFA_MTD_FLAG_TYPE_MASK
+ *     Bit-mask to extract memory management transaction type from flags.
+ * * @FFA_MTD_FLAG_TYPE_SHARE_MEMORY
+ *     Share memory transaction flag.
+ *     Used by @SMC_FC_FFA_MEM_RETRIEVE_RESP to indicate that memory came from
+ *     @SMC_FC_FFA_MEM_SHARE and by @SMC_FC_FFA_MEM_RETRIEVE_REQ to specify that
+ *     it must have.
+ * * @FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK
+ *     Not supported by this implementation.
+ */
+typedef uint32_t ffa_mtd_flag32_t;
+#define FFA_MTD_FLAG_ZERO_MEMORY			(1U << 0)
+#define FFA_MTD_FLAG_TIME_SLICING			(1U << 1)
+#define FFA_MTD_FLAG_ZERO_MEMORY_AFTER_RELINQUISH	(1U << 2)
+#define FFA_MTD_FLAG_TYPE_MASK				(3U << 3)
+#define FFA_MTD_FLAG_TYPE_SHARE_MEMORY			(1U << 3)
+#define FFA_MTD_FLAG_TYPE_LEND_MEMORY			(1U << 4)
+#define FFA_MTD_FLAG_ADDRESS_RANGE_ALIGNMENT_HINT_MASK	(0x1FU << 5)
+
+/**
+ * struct ffa_mapd - Memory access permissions descriptor
+ * @endpoint_id:
+ *         Endpoint id that @memory_access_permissions and @flags apply to.
+ *         (&typedef ffa_endpoint_id16_t).
+ * @memory_access_permissions:
+ *         FFA_MEM_PERM_* values or'ed together (&typedef ffa_mem_perm8_t).
+ * @flags:
+ *         FFA_MEM_FLAG_* values or'ed together (&typedef ffa_mem_flag8_t).
+ */
+struct ffa_mapd {
+	ffa_endpoint_id16_t endpoint_id;
+	ffa_mem_perm8_t memory_access_permissions;
+	ffa_mem_flag8_t flags;
+};
+CASSERT(sizeof(struct ffa_mapd) == 4, assert_ffa_mapd_size_mismatch);
+
+/**
+ * struct ffa_emad_v1_0 - Endpoint memory access descriptor.
+ * @mapd:  &struct ffa_mapd.
+ * @comp_mrd_offset:
+ *         Offset of &struct ffa_comp_mrd from start of &struct ffa_mtd_v1_0.
+ * @reserved_8_15:
+ *         Reserved bytes 8-15. Must be 0.
+ */
+struct ffa_emad_v1_0 {
+	struct ffa_mapd mapd;
+	uint32_t comp_mrd_offset;
+	uint64_t reserved_8_15;
+};
+CASSERT(sizeof(struct ffa_emad_v1_0) == 16, assert_ffa_emad_v1_0_size_mismatch);
+
+/**
+ * struct ffa_mtd_v1_0 - Memory transaction descriptor.
+ * @sender_id:
+ *         Sender endpoint id.
+ * @memory_region_attributes:
+ *         FFA_MEM_ATTR_* values or'ed together (&typedef ffa_mem_attr8_t).
+ * @reserved_3:
+ *         Reserved bytes 3. Must be 0.
+ * @flags:
+ *         FFA_MTD_FLAG_* values or'ed together (&typedef ffa_mtd_flag32_t).
+ * @handle:
+ *         Id of shared memory object. Must be 0 for MEM_SHARE or MEM_LEND.
+ * @tag:   Client allocated tag. Must match original value.
+ * @reserved_24_27:
+ *         Reserved bytes 24-27. Must be 0.
+ * @emad_count:
+ *         Number of entries in @emad.
+ * @emad:
+ *         Endpoint memory access descriptor array (see @struct ffa_emad_v1_0).
+ */
+struct ffa_mtd_v1_0 {
+	ffa_endpoint_id16_t sender_id;
+	ffa_mem_attr8_t memory_region_attributes;
+	uint8_t reserved_3;
+	ffa_mtd_flag32_t flags;
+	uint64_t handle;
+	uint64_t tag;
+	uint32_t reserved_24_27;
+	uint32_t emad_count;
+	struct ffa_emad_v1_0 emad[];
+};
+CASSERT(sizeof(struct ffa_mtd_v1_0) == 32, assert_ffa_mtd_size_v1_0_mismatch);
+
+/**
+ * struct ffa_mtd - Memory transaction descriptor for FF-A v1.1.
+ * @sender_id:
+ *         Sender endpoint id.
+ * @memory_region_attributes:
+ *         FFA_MEM_ATTR_* values or'ed together (&typedef ffa_mem_attr16_t).
+ * @flags:
+ *         FFA_MTD_FLAG_* values or'ed together (&typedef ffa_mtd_flag32_t).
+ * @handle:
+ *         Id of shared memory object. Must be 0 for MEM_SHARE or MEM_LEND.
+ * @tag:   Client allocated tag. Must match original value.
+ * @emad_size:
+ *         Size of the emad descriptor.
+ * @emad_count:
+ *         Number of entries in the emad array.
+ * @emad_offset:
+ *         Offset from the beginning of the descriptor to the location of the
+ *         memory access descriptor array (see @struct ffa_emad_v1_0).
+ * @reserved_36_39:
+ *         Reserved bytes 36-39. Must be 0.
+ * @reserved_40_47:
+ *         Reserved bytes 44-47. Must be 0.
+ */
+struct ffa_mtd {
+	ffa_endpoint_id16_t sender_id;
+	ffa_mem_attr16_t memory_region_attributes;
+	ffa_mtd_flag32_t flags;
+	uint64_t handle;
+	uint64_t tag;
+	uint32_t emad_size;
+	uint32_t emad_count;
+	uint32_t emad_offset;
+	uint32_t reserved_36_39;
+	uint64_t reserved_40_47;
+};
+CASSERT(sizeof(struct ffa_mtd) == 48, assert_ffa_mtd_size_mismatch);
+
+#endif /* EL3_SPMC_FFA_MEM_H */
diff --git a/include/services/ffa_svc.h b/include/services/ffa_svc.h
index 0836579..da016fd 100644
--- a/include/services/ffa_svc.h
+++ b/include/services/ffa_svc.h
@@ -56,6 +56,19 @@
 	(((blk) & FFA_MSG_SEND_ATTRS_BLK_MASK) \
 	<< FFA_MSG_SEND_ATTRS_BLK_SHIFT)
 
+/* Defines for FF-A framework messages exchanged using direct messages. */
+#define FFA_FWK_MSG_BIT		BIT(31)
+#define FFA_FWK_MSG_MASK	0xFF
+#define FFA_FWK_MSG_PSCI	U(0x0)
+
+/* Defines for FF-A power management messages framework messages. */
+#define FFA_PM_MSG_WB_REQ	U(0x1) /* Warm boot request. */
+#define FFA_PM_MSG_PM_RESP	U(0x2) /* Response to PSCI or warmboot req. */
+
+/* FF-A warm boot types. */
+#define FFA_WB_TYPE_S2RAM	0x0
+#define FFA_WB_TYPE_NOTS2RAM	0x1
+
 /* Get FFA fastcall std FID from function number */
 #define FFA_FID(smc_cc, func_num)			\
 		((SMC_TYPE_FAST << FUNCID_TYPE_SHIFT) |	\
@@ -88,6 +101,8 @@
 #define FFA_FNUM_MEM_RETRIEVE_RESP		U(0x75)
 #define FFA_FNUM_MEM_RELINQUISH			U(0x76)
 #define FFA_FNUM_MEM_RECLAIM			U(0x77)
+#define FFA_FNUM_MEM_FRAG_RX			U(0x7A)
+#define FFA_FNUM_MEM_FRAG_TX			U(0x7B)
 #define FFA_FNUM_NORMAL_WORLD_RESUME		U(0x7C)
 
 /* FF-A v1.1 */
@@ -143,6 +158,8 @@
 #define FFA_NOTIFICATION_GET 	FFA_FID(SMC_32, FFA_FNUM_NOTIFICATION_GET)
 #define FFA_NOTIFICATION_INFO_GET \
 	FFA_FID(SMC_32, FFA_FNUM_NOTIFICATION_INFO_GET)
+#define FFA_MEM_FRAG_RX	FFA_FID(SMC_32, FFA_FNUM_MEM_FRAG_RX)
+#define FFA_MEM_FRAG_TX	FFA_FID(SMC_32, FFA_FNUM_MEM_FRAG_TX)
 #define FFA_SPM_ID_GET		FFA_FID(SMC_32, FFA_FNUM_SPM_ID_GET)
 #define FFA_NORMAL_WORLD_RESUME	FFA_FID(SMC_32, FFA_FNUM_NORMAL_WORLD_RESUME)
 
@@ -196,6 +213,11 @@
 #define SPMC_SECURE_ID_SHIFT			U(15)
 
 /*
+ * Partition Count Flag in FFA_PARTITION_INFO_GET.
+ */
+#define FFA_PARTITION_INFO_GET_COUNT_FLAG_MASK U(1 << 0)
+
+/*
  * Mask for source and destination endpoint id in
  * a direct message request/response.
  */
@@ -249,4 +271,71 @@
 	return !ffa_is_secure_world_id(id);
 }
 
+
+/******************************************************************************
+ * Boot information protocol as per the FF-A v1.1 spec.
+ *****************************************************************************/
+#define FFA_INIT_DESC_SIGNATURE			0x00000FFA
+
+/* Boot information type. */
+#define FFA_BOOT_INFO_TYPE_STD			U(0x0)
+#define FFA_BOOT_INFO_TYPE_IMPL			U(0x1)
+
+#define FFA_BOOT_INFO_TYPE_MASK			U(0x1)
+#define FFA_BOOT_INFO_TYPE_SHIFT		U(0x7)
+#define FFA_BOOT_INFO_TYPE(type)		\
+	(((type) & FFA_BOOT_INFO_TYPE_MASK)	\
+	<< FFA_BOOT_INFO_TYPE_SHIFT)
+
+/* Boot information identifier. */
+#define FFA_BOOT_INFO_TYPE_ID_FDT		U(0x0)
+#define FFA_BOOT_INFO_TYPE_ID_HOB		U(0x1)
+
+#define FFA_BOOT_INFO_TYPE_ID_MASK		U(0x3F)
+#define FFA_BOOT_INFO_TYPE_ID_SHIFT		U(0x0)
+#define FFA_BOOT_INFO_TYPE_ID(type)		\
+	(((type) & FFA_BOOT_INFO_TYPE_ID_MASK)	\
+	<< FFA_BOOT_INFO_TYPE_ID_SHIFT)
+
+/* Format of Flags Name field. */
+#define FFA_BOOT_INFO_FLAG_NAME_STRING		U(0x0)
+#define FFA_BOOT_INFO_FLAG_NAME_UUID		U(0x1)
+
+#define FFA_BOOT_INFO_FLAG_NAME_MASK		U(0x3)
+#define FFA_BOOT_INFO_FLAG_NAME_SHIFT		U(0x0)
+#define FFA_BOOT_INFO_FLAG_NAME(type)		\
+	(((type) & FFA_BOOT_INFO_FLAG_NAME_MASK)\
+	<< FFA_BOOT_INFO_FLAG_NAME_SHIFT)
+
+/* Format of Flags Contents field. */
+#define FFA_BOOT_INFO_FLAG_CONTENT_ADR		U(0x0)
+#define FFA_BOOT_INFO_FLAG_CONTENT_VAL		U(0x1)
+
+#define FFA_BOOT_INFO_FLAG_CONTENT_MASK		U(0x1)
+#define FFA_BOOT_INFO_FLAG_CONTENT_SHIFT	U(0x2)
+#define FFA_BOOT_INFO_FLAG_CONTENT(content)		\
+	(((content) & FFA_BOOT_INFO_FLAG_CONTENT_MASK)	\
+	<< FFA_BOOT_INFO_FLAG_CONTENT_SHIFT)
+
+/* Boot information descriptor. */
+struct ffa_boot_info_desc {
+	uint8_t name[16];
+	uint8_t type;
+	uint8_t reserved;
+	uint16_t flags;
+	uint32_t size_boot_info;
+	uint64_t content;
+};
+
+/* Boot information header. */
+struct ffa_boot_info_header {
+	uint32_t signature; /* 0xFFA */
+	uint32_t version;
+	uint32_t size_boot_info_blob;
+	uint32_t size_boot_info_desc;
+	uint32_t count_boot_info_desc;
+	uint32_t offset_boot_info_desc;
+	uint64_t reserved;
+};
+
 #endif /* FFA_SVC_H */
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 0f09ebe..e393493 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -36,6 +36,64 @@
 #endif /* ENABLE_FEAT_TWED */
 
 static void manage_extensions_secure(cpu_context_t *ctx);
+
+static void setup_el1_context(cpu_context_t *ctx, const struct entry_point_info *ep)
+{
+	u_register_t sctlr_elx, actlr_elx;
+
+	/*
+	 * Initialise SCTLR_EL1 to the reset value corresponding to the target
+	 * execution state setting all fields rather than relying on the hw.
+	 * Some fields have architecturally UNKNOWN reset values and these are
+	 * set to zero.
+	 *
+	 * SCTLR.EE: Endianness is taken from the entrypoint attributes.
+	 *
+	 * SCTLR.M, SCTLR.C and SCTLR.I: These fields must be zero (as
+	 * required by PSCI specification)
+	 */
+	sctlr_elx = (EP_GET_EE(ep->h.attr) != 0U) ? SCTLR_EE_BIT : 0UL;
+	if (GET_RW(ep->spsr) == MODE_RW_64) {
+		sctlr_elx |= SCTLR_EL1_RES1;
+	} else {
+		/*
+		 * If the target execution state is AArch32 then the following
+		 * fields need to be set.
+		 *
+		 * SCTRL_EL1.nTWE: Set to one so that EL0 execution of WFE
+		 *  instructions are not trapped to EL1.
+		 *
+		 * SCTLR_EL1.nTWI: Set to one so that EL0 execution of WFI
+		 *  instructions are not trapped to EL1.
+		 *
+		 * SCTLR_EL1.CP15BEN: Set to one to enable EL0 execution of the
+		 *  CP15DMB, CP15DSB, and CP15ISB instructions.
+		 */
+		sctlr_elx |= SCTLR_AARCH32_EL1_RES1 | SCTLR_CP15BEN_BIT
+					| SCTLR_NTWI_BIT | SCTLR_NTWE_BIT;
+	}
+
+#if ERRATA_A75_764081
+	/*
+	 * If workaround of errata 764081 for Cortex-A75 is used then set
+	 * SCTLR_EL1.IESB to enable Implicit Error Synchronization Barrier.
+	 */
+	sctlr_elx |= SCTLR_IESB_BIT;
+#endif
+	/* Store the initialised SCTLR_EL1 value in the cpu_context */
+	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
+	 * implementation defined. The context restore process will write
+	 * the value from the context to the actual register and can cause
+	 * problems for processor cores that don't expect certain bits to
+	 * be zero.
+	 */
+	actlr_elx = read_actlr_el1();
+	write_ctx_reg((get_el1_sysregs_ctx(ctx)), (CTX_ACTLR_EL1), (actlr_elx));
+}
+
 /******************************************************************************
  * This function performs initializations that are specific to SECURE state
  * and updates the cpu context specified by 'ctx'.
@@ -85,6 +143,14 @@
 
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 
+	/*
+	 * Initialize EL1 context registers unless SPMC is running
+	 * at S-EL2.
+	 */
+#if !SPMD_SPM_AT_SEL2
+	setup_el1_context(ctx, ep);
+#endif
+
 	manage_extensions_secure(ctx);
 }
 
@@ -147,6 +213,9 @@
 #endif
 	write_ctx_reg(state, CTX_SCR_EL3, scr_el3);
 
+	/* Initialize EL1 context registers */
+	setup_el1_context(ctx, ep);
+
 	/* Initialize EL2 context registers */
 #if CTX_INCLUDE_EL2_REGS
 
@@ -186,7 +255,6 @@
 	u_register_t scr_el3;
 	el3_state_t *state;
 	gp_regs_t *gp_regs;
-	u_register_t sctlr_elx, actlr_elx;
 
 	/* Clear any residual register values from the context */
 	zeromem(ctx, sizeof(*ctx));
@@ -214,8 +282,10 @@
 
 	/*
 	 * SCR_EL3.ST: Traps Secure EL1 accesses to the Counter-timer Physical
-	 *  Secure timer registers to EL3, from AArch64 state only, if specified
-	 *  by the entrypoint attributes.
+	 * Secure timer registers to EL3, from AArch64 state only, if specified
+	 * by the entrypoint attributes. If SEL2 is present and enabled, the ST
+	 * bit always behaves as 1 (i.e. secure physical timer register access
+	 * is not trapped)
 	 */
 	if (EP_GET_ST(ep->h.attr) != 0U) {
 		scr_el3 |= SCR_ST_BIT;
@@ -283,46 +353,6 @@
 		}
 	}
 
-	/*
-	 * Initialise SCTLR_EL1 to the reset value corresponding to the target
-	 * execution state setting all fields rather than relying of the hw.
-	 * Some fields have architecturally UNKNOWN reset values and these are
-	 * set to zero.
-	 *
-	 * SCTLR.EE: Endianness is taken from the entrypoint attributes.
-	 *
-	 * SCTLR.M, SCTLR.C and SCTLR.I: These fields must be zero (as
-	 *  required by PSCI specification)
-	 */
-	sctlr_elx = (EP_GET_EE(ep->h.attr) != 0U) ? SCTLR_EE_BIT : 0U;
-	if (GET_RW(ep->spsr) == MODE_RW_64) {
-		sctlr_elx |= SCTLR_EL1_RES1;
-	} else {
-		/*
-		 * If the target execution state is AArch32 then the following
-		 * fields need to be set.
-		 *
-		 * SCTRL_EL1.nTWE: Set to one so that EL0 execution of WFE
-		 *  instructions are not trapped to EL1.
-		 *
-		 * SCTLR_EL1.nTWI: Set to one so that EL0 execution of WFI
-		 *  instructions are not trapped to EL1.
-		 *
-		 * SCTLR_EL1.CP15BEN: Set to one to enable EL0 execution of the
-		 *  CP15DMB, CP15DSB, and CP15ISB instructions.
-		 */
-		sctlr_elx |= SCTLR_AARCH32_EL1_RES1 | SCTLR_CP15BEN_BIT
-					| SCTLR_NTWI_BIT | SCTLR_NTWE_BIT;
-	}
-
-#if ERRATA_A75_764081
-	/*
-	 * If workaround of errata 764081 for Cortex-A75 is used then set
-	 * SCTLR_EL1.IESB to enable Implicit Error Synchronization Barrier.
-	 */
-	sctlr_elx |= SCTLR_IESB_BIT;
-#endif
-
 #if ENABLE_FEAT_TWED
 	/* Enable WFE trap delay in SCR_EL3 if supported and configured */
 	/* Set delay in SCR_EL3 */
@@ -335,23 +365,6 @@
 #endif /* ENABLE_FEAT_TWED */
 
 	/*
-	 * Store the initialised SCTLR_EL1 value in the cpu_context - SCTLR_EL2
-	 * and other EL2 registers are set up by cm_prepare_el3_exit() as they
-	 * are not part of the stored cpu_context.
-	 */
-	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
-	 * implementation defined. The context restore process will write
-	 * the value from the context to the actual register and can cause
-	 * problems for processor cores that don't expect certain bits to
-	 * be zero.
-	 */
-	actlr_elx = read_actlr_el1();
-	write_ctx_reg((get_el1_sysregs_ctx(ctx)), (CTX_ACTLR_EL1), (actlr_elx));
-
-	/*
 	 * Populate EL3 state so that we've the right context
 	 * before doing ERET
 	 */
@@ -820,6 +833,14 @@
 	cpu_context_t *ctx = cm_get_context(NON_SECURE);
 	assert(ctx != NULL);
 
+	/* Assert that EL2 is used. */
+#if ENABLE_ASSERTIONS
+	el3_state_t *state = get_el3state_ctx(ctx);
+	u_register_t scr_el3 = read_ctx_reg(state, CTX_SCR_EL3);
+#endif
+	assert(((scr_el3 & SCR_HCE_BIT) != 0UL) &&
+			(el_implemented(2U) != EL_IMPL_NONE));
+
 	/*
 	 * Currently some extensions are configured using
 	 * direct register updates. Therefore, do this here
diff --git a/make_helpers/defaults.mk b/make_helpers/defaults.mk
index d5383a1..fab6bf6 100644
--- a/make_helpers/defaults.mk
+++ b/make_helpers/defaults.mk
@@ -463,3 +463,6 @@
 
 # By default, disable the mocking of RSS provided services
 PLAT_RSS_NOT_SUPPORTED		:= 0
+
+# Dynamic Root of Trust for Measurement support
+DRTM_SUPPORT			:= 0
diff --git a/package-lock.json b/package-lock.json
index 469c5f5..4284d71 100644
--- a/package-lock.json
+++ b/package-lock.json
@@ -843,9 +843,9 @@
       }
     },
     "node_modules/commitizen/node_modules/ansi-regex": {
-      "version": "4.1.0",
-      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.0.tgz",
-      "integrity": "sha512-1apePfXM1UOSqw0o9IiFAovVz9M5S1Dg+4TrDwfMewQ6p/rmMueb7tWZjQ1rx4Loy1ArBggoqGpfqqdI4rondg==",
+      "version": "4.1.1",
+      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.1.tgz",
+      "integrity": "sha512-ILlv4k/3f6vfQ4OoP2AGvirOktlQ98ZEL1k9FaQjxa3L1abBgbuTDAdPOpvbGncC0BTVQrl+OM8xZGK6tWXt7g==",
       "dev": true,
       "engines": {
         "node": ">=6"
@@ -1073,9 +1073,9 @@
       }
     },
     "node_modules/commitizen/node_modules/string-width/node_modules/ansi-regex": {
-      "version": "3.0.0",
-      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.0.tgz",
-      "integrity": "sha1-7QMXwyIGT3lGbAKWa922Bas32Zg=",
+      "version": "3.0.1",
+      "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.1.tgz",
+      "integrity": "sha512-+O9Jct8wf++lXxxFc4hc8LsjaSq0HFzzL7cVsw8pRDIPdjKD2mT4ytDZlLuSBZ4cLKZFXIrMGO7DbQCtMJJMKw==",
       "dev": true,
       "engines": {
         "node": ">=4"
@@ -4792,9 +4792,9 @@
           "dev": true
         },
         "ansi-regex": {
-          "version": "4.1.0",
-          "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.0.tgz",
-          "integrity": "sha512-1apePfXM1UOSqw0o9IiFAovVz9M5S1Dg+4TrDwfMewQ6p/rmMueb7tWZjQ1rx4Loy1ArBggoqGpfqqdI4rondg==",
+          "version": "4.1.1",
+          "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-4.1.1.tgz",
+          "integrity": "sha512-ILlv4k/3f6vfQ4OoP2AGvirOktlQ98ZEL1k9FaQjxa3L1abBgbuTDAdPOpvbGncC0BTVQrl+OM8xZGK6tWXt7g==",
           "dev": true
         },
         "ansi-styles": {
@@ -4975,9 +4975,9 @@
           },
           "dependencies": {
             "ansi-regex": {
-              "version": "3.0.0",
-              "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.0.tgz",
-              "integrity": "sha1-7QMXwyIGT3lGbAKWa922Bas32Zg=",
+              "version": "3.0.1",
+              "resolved": "https://registry.npmjs.org/ansi-regex/-/ansi-regex-3.0.1.tgz",
+              "integrity": "sha512-+O9Jct8wf++lXxxFc4hc8LsjaSq0HFzzL7cVsw8pRDIPdjKD2mT4ytDZlLuSBZ4cLKZFXIrMGO7DbQCtMJJMKw==",
               "dev": true
             },
             "strip-ansi": {
diff --git a/plat/arm/board/fvp/fvp_el3_spmc.c b/plat/arm/board/fvp/fvp_el3_spmc.c
new file mode 100644
index 0000000..2b347ed
--- /dev/null
+++ b/plat/arm/board/fvp/fvp_el3_spmc.c
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <services/el3_spmc_ffa_memory.h>
+
+#include <platform_def.h>
+
+/*
+ * On the FVP platform when using the EL3 SPMC implementation allocate the
+ * datastore for tracking shared memory descriptors in the TZC DRAM section
+ * to ensure sufficient storage can be allocated.
+ * Provide an implementation of the accessor method to allow the datastore
+ * details to be retrieved by the SPMC.
+ * The SPMC will take care of initializing the memory region.
+ */
+
+#define PLAT_SPMC_SHMEM_DATASTORE_SIZE 512 * 1024
+
+__section("arm_el3_tzc_dram") static uint8_t
+plat_spmc_shmem_datastore[PLAT_SPMC_SHMEM_DATASTORE_SIZE];
+
+int plat_spmc_shmem_datastore_get(uint8_t **datastore, size_t *size)
+{
+	*datastore = plat_spmc_shmem_datastore;
+	*size = PLAT_SPMC_SHMEM_DATASTORE_SIZE;
+	return 0;
+}
+
+/*
+ * Add dummy implementations of memory management related platform hooks.
+ * These can be used to implement platform specific functionality to support
+ * a memory sharing/lending operation.
+ *
+ * Note: The hooks must be located as part of the initial share request and
+ * final reclaim to prevent order dependencies with operations that may take
+ * place in the normal world without visibility of the SPMC.
+ */
+int plat_spmc_shmem_begin(struct ffa_mtd *desc)
+{
+	return 0;
+}
+int plat_spmc_shmem_reclaim(struct ffa_mtd *desc)
+{
+	return 0;
+}
diff --git a/plat/arm/board/fvp/platform.mk b/plat/arm/board/fvp/platform.mk
index 89ca185..54c5e75 100644
--- a/plat/arm/board/fvp/platform.mk
+++ b/plat/arm/board/fvp/platform.mk
@@ -425,3 +425,7 @@
 
 # enable trace filter control registers access to NS by default
 ENABLE_TRF_FOR_NS		:= 1
+
+ifeq (${SPMC_AT_EL3}, 1)
+PLAT_BL_COMMON_SOURCES	+=	plat/arm/board/fvp/fvp_el3_spmc.c
+endif
diff --git a/plat/arm/board/n1sdp/fdts/n1sdp_fw_config.dts b/plat/arm/board/n1sdp/fdts/n1sdp_fw_config.dts
new file mode 100644
index 0000000..f61e30b
--- /dev/null
+++ b/plat/arm/board/n1sdp/fdts/n1sdp_fw_config.dts
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/tbbr/tbbr_img_def.h>
+
+/dts-v1/;
+/ {
+	dtb-registry {
+		compatible = "fconf,dyn_cfg-dtb_registry";
+		tb_fw-config {
+			load-address = <0x0 0x4001300>;
+			max-size = <0x200>;
+			id = <TB_FW_CONFIG_ID>;
+		};
+
+		nt_fw-config {
+			load-address = <0x0 0xFEF00000>;
+			max-size = <0x0100000>;
+			id = <NT_FW_CONFIG_ID>;
+		};
+	};
+};
diff --git a/plat/arm/board/n1sdp/fdts/n1sdp_nt_fw_config.dts b/plat/arm/board/n1sdp/fdts/n1sdp_nt_fw_config.dts
new file mode 100644
index 0000000..da5e04d
--- /dev/null
+++ b/plat/arm/board/n1sdp/fdts/n1sdp_nt_fw_config.dts
@@ -0,0 +1,23 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+/ {
+	/* compatible string */
+	compatible = "arm,n1sdp";
+
+	/*
+	 * Place holder for platform-info node with default values.
+	 * The values will be set to the correct values during
+	 * the BL2 stage of boot.
+	 */
+	platform-info {
+		multichip-mode = <0x0>;
+		secondary-chip-count = <0x0>;
+		local-ddr-size = <0x0>;
+		remote-ddr-size = <0x0>;
+	};
+};
\ No newline at end of file
diff --git a/plat/arm/board/n1sdp/fdts/n1sdp_tb_fw_config.dts b/plat/arm/board/n1sdp/fdts/n1sdp_tb_fw_config.dts
new file mode 100644
index 0000000..e5ffba3
--- /dev/null
+++ b/plat/arm/board/n1sdp/fdts/n1sdp_tb_fw_config.dts
@@ -0,0 +1,27 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/dts-v1/;
+/ {
+	tb_fw-config {
+		compatible = "arm,tb_fw";
+
+		/* Disable authentication for development */
+		disable_auth = <0x0>;
+
+		/*
+		 * The following two entries are placeholders for Mbed TLS
+		 * heap information. The default values don't matter since
+		 * they will be overwritten by BL1.
+		 * In case of having shared Mbed TLS heap between BL1 and BL2,
+		 * BL1 will populate these two properties with the respective
+		 * info about the shared heap. This info will be available for
+		 * BL2 in order to locate and re-use the heap.
+		 */
+		mbedtls_heap_addr = <0x0 0x0>;
+		mbedtls_heap_size = <0x0>;
+	};
+};
diff --git a/plat/arm/board/n1sdp/include/platform_def.h b/plat/arm/board/n1sdp/include/platform_def.h
index cc07852..c9b81ba 100644
--- a/plat/arm/board/n1sdp/include/platform_def.h
+++ b/plat/arm/board/n1sdp/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -27,6 +27,27 @@
 #define PLAT_ARM_DRAM2_BASE			ULL(0x8080000000)
 #define PLAT_ARM_DRAM2_SIZE			ULL(0xF80000000)
 
+#define MAX_IO_DEVICES			U(3)
+#define MAX_IO_HANDLES			U(4)
+
+#define PLAT_ARM_FLASH_IMAGE_BASE			0x18200000
+#define PLAT_ARM_FLASH_IMAGE_MAX_SIZE			0x00800000
+
+#define PLAT_ARM_NVM_BASE			0x18200000
+#define PLAT_ARM_NVM_SIZE			0x00800000
+
+#if defined NS_BL1U_BASE
+# undef NS_BL1U_BASE
+# define NS_BL1U_BASE			(PLAT_ARM_NVM_BASE + UL(0x00800000))
+#endif
+
+/* Non-volatile counters */
+#define SOC_TRUSTED_NVCTR_BASE		0x7fe70000
+#define TFW_NVCTR_BASE			(SOC_TRUSTED_NVCTR_BASE)
+#define TFW_NVCTR_SIZE			U(4)
+#define NTFW_CTR_BASE			(SOC_TRUSTED_NVCTR_BASE + 0x0004)
+#define NTFW_CTR_SIZE			U(4)
+
 /* N1SDP remote chip at 4 TB offset */
 #define PLAT_ARM_REMOTE_CHIP_OFFSET		(ULL(1) << 42)
 
@@ -59,8 +80,42 @@
 #define PLAT_CSS_SCP_COM_SHARED_MEM_BASE	0x45400000
 #endif
 
+/*
+ * Trusted SRAM in N1SDP is 512 KB but only the bottom 384 KB
+ * is used for trusted board boot flow. The top 128 KB is used
+ * to load AP-BL1 image.
+ */
+#define PLAT_ARM_TRUSTED_SRAM_SIZE                      0x00060000      /* 384 KB */
+
-#define PLAT_ARM_TRUSTED_SRAM_SIZE		0x00080000	/* 512 KB */
-#define PLAT_ARM_MAX_BL31_SIZE			0X20000
+/*
+ * PLAT_ARM_MAX_BL1_RW_SIZE is calculated using the current BL1 RW debug size
+ * plus a little space for growth.
+ */
+#define PLAT_ARM_MAX_BL1_RW_SIZE	0xE000
+
+/*
+ * PLAT_ARM_MAX_ROMLIB_RW_SIZE is define to use a full page
+ */
+
+#if USE_ROMLIB
+# define PLAT_ARM_MAX_ROMLIB_RW_SIZE	0x1000
+# define PLAT_ARM_MAX_ROMLIB_RO_SIZE	0xe000
+#else
+# define PLAT_ARM_MAX_ROMLIB_RW_SIZE	U(0)
+# define PLAT_ARM_MAX_ROMLIB_RO_SIZE	U(0)
+#endif
+
+/*
+ * PLAT_ARM_MAX_BL2_SIZE is calculated using the current BL2 debug size plus a
+ * little space for growth.
+ */
+#if TRUSTED_BOARD_BOOT
+# define PLAT_ARM_MAX_BL2_SIZE		0x20000
+#else
+# define PLAT_ARM_MAX_BL2_SIZE		0x14000
+#endif
+
+#define PLAT_ARM_MAX_BL31_SIZE		UL(0x3B000)
 
 /*******************************************************************************
  * N1SDP topology related constants
@@ -83,10 +138,48 @@
  * PLAT_ARM_MMAP_ENTRIES depends on the number of entries in the
  * plat_arm_mmap array defined for each BL stage.
  */
-#define PLAT_ARM_MMAP_ENTRIES			9
-#define MAX_XLAT_TABLES				10
+
+#ifdef IMAGE_BL1
+# define PLAT_ARM_MMAP_ENTRIES		U(6)
+# define MAX_XLAT_TABLES		U(5)
+#endif
 
-#define PLATFORM_STACK_SIZE			0x400
+#ifdef IMAGE_BL2
+#  define PLAT_ARM_MMAP_ENTRIES		U(11)
+#  define MAX_XLAT_TABLES		U(10)
+#endif
+
+#ifdef IMAGE_BL31
+#  define PLAT_ARM_MMAP_ENTRIES		U(12)
+#  define MAX_XLAT_TABLES		U(12)
+#endif
+
+/*
+ * Size of cacheable stacks
+ */
+#if defined(IMAGE_BL1)
+# if TRUSTED_BOARD_BOOT
+#  define PLATFORM_STACK_SIZE	0x1000
+# else
+#  define PLATFORM_STACK_SIZE	0x440
+# endif
+#elif defined(IMAGE_BL2)
+# if TRUSTED_BOARD_BOOT
+#  define PLATFORM_STACK_SIZE	0x1000
+# else
+#  define PLATFORM_STACK_SIZE	0x400
+# endif
+#elif defined(IMAGE_BL2U)
+# define PLATFORM_STACK_SIZE	0x400
+#elif defined(IMAGE_BL31)
+# if SPM_MM
+#  define PLATFORM_STACK_SIZE	0x500
+# else
+#  define PLATFORM_STACK_SIZE	0x400
+# endif
+#elif defined(IMAGE_BL32)
+# define PLATFORM_STACK_SIZE	0x440
+#endif
 
 #define PLAT_ARM_NSTIMER_FRAME_ID		0
 #define PLAT_CSS_MHU_BASE			0x45000000
@@ -106,6 +199,10 @@
 						PLAT_ARM_REMOTE_CHIP_OFFSET
 #define N1SDP_REMOTE_DEVICE_SIZE		N1SDP_DEVICE_SIZE
 
+/* Real base is 0x0. Changed to load BL1 at this address */
+# define PLAT_ARM_TRUSTED_ROM_BASE	0x04060000
+# define PLAT_ARM_TRUSTED_ROM_SIZE	0x00020000	/* 128KB */
+
 #define N1SDP_MAP_DEVICE		MAP_REGION_FLAT(	\
 					N1SDP_DEVICE_BASE,	\
 					N1SDP_DEVICE_SIZE,	\
diff --git a/plat/arm/board/n1sdp/n1sdp_bl1_setup.c b/plat/arm/board/n1sdp/n1sdp_bl1_setup.c
new file mode 100644
index 0000000..ed93222
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_bl1_setup.c
@@ -0,0 +1,19 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*******************************************************************************
+ * Perform any BL1 specific platform actions.
+ ******************************************************************************/
+
+void soc_css_init_nic400(void)
+{
+}
+
+void soc_css_init_pcie(void)
+{
+}
diff --git a/plat/arm/board/n1sdp/n1sdp_bl2_setup.c b/plat/arm/board/n1sdp/n1sdp_bl2_setup.c
new file mode 100644
index 0000000..5f8af9f
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_bl2_setup.c
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <drivers/arm/css/sds.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#include "n1sdp_def.h"
+#include <plat/arm/common/plat_arm.h>
+
+struct n1sdp_plat_info {
+	bool multichip_mode;
+	uint8_t secondary_count;
+	uint8_t local_ddr_size;
+	uint8_t remote_ddr_size;
+} __packed;
+
+/*
+ * N1SDP platform supports RDIMMs with ECC capability. To use the ECC
+ * capability, the entire DDR memory space has to be zeroed out before
+ * enabling the ECC bits in DMC620. Zeroing out several gigabytes of
+ * memory from SCP is quite time consuming so the following function
+ * is added to zero out the DDR memory from application processor which is
+ * much faster compared to SCP.
+ */
+
+void dmc_ecc_setup(uint8_t ddr_size_gb)
+{
+	uint64_t dram2_size;
+
+	dram2_size = (ddr_size_gb * 1024UL * 1024UL * 1024UL) -
+			ARM_DRAM1_SIZE;
+
+	INFO("Zeroing DDR memories\n");
+	zero_normalmem((void *)ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
+	flush_dcache_range(ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
+	zero_normalmem((void *)ARM_DRAM2_BASE, dram2_size);
+	flush_dcache_range(ARM_DRAM2_BASE, dram2_size);
+
+	INFO("Enabling ECC on DMCs\n");
+	/* Set DMCs to CONFIG state before writing ERR0CTLR0 register */
+	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
+	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
+
+	/* Enable ECC in DMCs */
+	mmio_setbits_32(N1SDP_DMC0_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
+	mmio_setbits_32(N1SDP_DMC1_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
+
+	/* Set DMCs to READY state */
+	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
+	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
+}
+
+void bl2_platform_setup(void)
+{
+	int ret;
+	struct n1sdp_plat_info plat_info;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SDS initialization failed\n");
+		panic();
+	}
+
+	ret = sds_struct_read(N1SDP_SDS_PLATFORM_INFO_STRUCT_ID,
+				N1SDP_SDS_PLATFORM_INFO_OFFSET,
+				&plat_info,
+				N1SDP_SDS_PLATFORM_INFO_SIZE,
+				SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK) {
+		ERROR("Error getting platform info from SDS\n");
+		panic();
+	}
+	/* Validate plat_info SDS */
+	if ((plat_info.local_ddr_size == 0)
+		|| (plat_info.local_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.remote_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.secondary_count > N1SDP_MAX_SECONDARY_COUNT)) {
+		ERROR("platform info SDS is corrupted\n");
+		panic();
+	}
+
+	dmc_ecc_setup(plat_info.local_ddr_size);
+	arm_bl2_platform_setup();
+}
diff --git a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
index d7003e9..5e897fe 100644
--- a/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
+++ b/plat/arm/board/n1sdp/n1sdp_bl31_setup.c
@@ -1,11 +1,9 @@
 /*
- * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <platform_def.h>
-
 #include <common/debug.h>
 #include <drivers/arm/css/css_mhu_doorbell.h>
 #include <drivers/arm/css/scmi.h>
@@ -16,6 +14,7 @@
 #include <plat/arm/common/plat_arm.h>
 
 #include "n1sdp_def.h"
+#include <platform_def.h>
 
 /*
  * Platform information structure stored in SDS.
@@ -24,28 +23,17 @@
  * enabling the ECC capability as well as information
  * about multichip setup
  * 	- multichip mode
- * 	- slave_count
+ * 	- secondary_count
  * 	- Local DDR size in GB, DDR memory in master board
- * 	- Remote DDR size in GB, DDR memory in slave board
+ * 	- Remote DDR size in GB, DDR memory in secondary board
  */
 struct n1sdp_plat_info {
 	bool multichip_mode;
-	uint8_t slave_count;
+	uint8_t secondary_count;
 	uint8_t local_ddr_size;
 	uint8_t remote_ddr_size;
 } __packed;
 
-/*
- * BL33 image information structure stored in SDS.
- * This structure holds the source & destination addresses and
- * the size of the BL33 image which will be loaded by BL31.
- */
-struct n1sdp_bl33_info {
-	uint32_t bl33_src_addr;
-	uint32_t bl33_dst_addr;
-	uint32_t bl33_size;
-};
-
 static scmi_channel_plat_info_t n1sdp_scmi_plat_info = {
 	.scmi_mbx_mem = N1SDP_SCMI_PAYLOAD_BASE,
 	.db_reg_addr = PLAT_CSS_MHU_BASE + CSS_SCMI_MHU_DB_REG_OFF,
@@ -90,38 +78,10 @@
  * enabling the ECC bits in DMC620. Zeroing out several gigabytes of
  * memory from SCP is quite time consuming so the following function
  * is added to zero out the DDR memory from application processor which is
- * much faster compared to SCP. BL33 binary cannot be copied to DDR memory
- * before enabling ECC so copy_bl33 function is added to copy BL33 binary
- * from IOFPGA-DDR3 memory to main DDR4 memory.
+ * much faster compared to SCP. Local DDR memory is zeroed out during BL2
+ * stage. If remote chip is connected, it's DDR memory is zeroed out here.
  */
 
-void dmc_ecc_setup(uint8_t ddr_size_gb)
-{
-	uint64_t dram2_size;
-
-	dram2_size = (ddr_size_gb * 1024UL * 1024UL * 1024UL) -
-			ARM_DRAM1_SIZE;
-
-	INFO("Zeroing DDR memories\n");
-	zero_normalmem((void *)ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
-	flush_dcache_range(ARM_DRAM1_BASE, ARM_DRAM1_SIZE);
-	zero_normalmem((void *)ARM_DRAM2_BASE, dram2_size);
-	flush_dcache_range(ARM_DRAM2_BASE, dram2_size);
-
-	INFO("Enabling ECC on DMCs\n");
-	/* Set DMCs to CONFIG state before writing ERR0CTLR0 register */
-	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
-	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_CONFIG);
-
-	/* Enable ECC in DMCs */
-	mmio_setbits_32(N1SDP_DMC0_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
-	mmio_setbits_32(N1SDP_DMC1_ERR0CTLR0_REG, N1SDP_DMC_ERR0CTLR0_ECC_EN);
-
-	/* Set DMCs to READY state */
-	mmio_write_32(N1SDP_DMC0_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
-	mmio_write_32(N1SDP_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
-}
-
 void remote_dmc_ecc_setup(uint8_t remote_ddr_size)
 {
 	uint64_t remote_dram2_size;
@@ -154,22 +114,6 @@
 	mmio_write_32(N1SDP_REMOTE_DMC1_MEMC_CMD_REG, N1SDP_DMC_MEMC_CMD_READY);
 }
 
-void copy_bl33(uint32_t src, uint32_t dst, uint32_t size)
-{
-	uint32_t i;
-
-	INFO("Copying BL33 to DDR memory\n");
-	for (i = 0; i < size; i = i + 8)
-		mmio_write_64((dst + i), mmio_read_64(src + i));
-
-	for (i = 0; i < size; i = i + 8) {
-		if (mmio_read_64(src + i) != mmio_read_64(dst + i)) {
-			ERROR("Copy failed!\n");
-			panic();
-		}
-	}
-}
-
 void n1sdp_bl31_multichip_setup(void)
 {
 	plat_arm_override_gicr_frames(n1sdp_multichip_gicr_frames);
@@ -180,7 +124,6 @@
 {
 	int ret;
 	struct n1sdp_plat_info plat_info;
-	struct n1sdp_bl33_info bl33_info;
 
 	ret = sds_init();
 	if (ret != SDS_OK) {
@@ -201,41 +144,18 @@
 	if ((plat_info.local_ddr_size == 0)
 		|| (plat_info.local_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
 		|| (plat_info.remote_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
-		|| (plat_info.slave_count > N1SDP_MAX_SLAVE_COUNT)) {
+		|| (plat_info.secondary_count > N1SDP_MAX_SECONDARY_COUNT)) {
 		ERROR("platform info SDS is corrupted\n");
 		panic();
 	}
 
 	if (plat_info.multichip_mode) {
-		n1sdp_multichip_data.chip_count = plat_info.slave_count + 1;
+		n1sdp_multichip_data.chip_count = plat_info.secondary_count + 1;
 		n1sdp_bl31_multichip_setup();
 	}
 	arm_bl31_platform_setup();
 
-	dmc_ecc_setup(plat_info.local_ddr_size);
-
 	/* Check if remote memory is present */
 	if ((plat_info.multichip_mode) && (plat_info.remote_ddr_size != 0))
 		remote_dmc_ecc_setup(plat_info.remote_ddr_size);
-
-	ret = sds_struct_read(N1SDP_SDS_BL33_INFO_STRUCT_ID,
-				N1SDP_SDS_BL33_INFO_OFFSET,
-				&bl33_info,
-				N1SDP_SDS_BL33_INFO_SIZE,
-				SDS_ACCESS_MODE_NON_CACHED);
-	if (ret != SDS_OK) {
-		ERROR("Error getting BL33 info from SDS\n");
-		panic();
-	}
-	copy_bl33(bl33_info.bl33_src_addr,
-			bl33_info.bl33_dst_addr,
-			bl33_info.bl33_size);
-	/*
-	 * Pass platform information to BL33. This method is followed as
-	 * currently there is no BL1/BL2 involved in boot flow of N1SDP.
-	 * When TBBR is implemented for N1SDP, this method should be removed
-	 * and platform information should be passed to BL33 using NT_FW_CONFIG
-	 * passing mechanism.
-	 */
-	mmio_write_32(N1SDP_PLATFORM_INFO_BASE, *(uint32_t *)&plat_info);
 }
diff --git a/plat/arm/board/n1sdp/n1sdp_def.h b/plat/arm/board/n1sdp/n1sdp_def.h
index 30e29a7..ffa6a03 100644
--- a/plat/arm/board/n1sdp/n1sdp_def.h
+++ b/plat/arm/board/n1sdp/n1sdp_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -20,12 +20,7 @@
 #define N1SDP_SDS_PLATFORM_INFO_OFFSET		0
 #define N1SDP_SDS_PLATFORM_INFO_SIZE		4
 #define N1SDP_MAX_DDR_CAPACITY_GB		64
-#define N1SDP_MAX_SLAVE_COUNT			16
-
-/* SDS BL33 image information defines */
-#define N1SDP_SDS_BL33_INFO_STRUCT_ID		9
-#define N1SDP_SDS_BL33_INFO_OFFSET		0
-#define N1SDP_SDS_BL33_INFO_SIZE		12
+#define N1SDP_MAX_SECONDARY_COUNT		16
 
 /* DMC memory command registers */
 #define N1SDP_DMC0_MEMC_CMD_REG			0x4E000008
@@ -54,7 +49,4 @@
 /* DMC ECC enable bit in ERR0CTLR0 register */
 #define N1SDP_DMC_ERR0CTLR0_ECC_EN		0x1
 
-/* Base address of non-secure SRAM where Platform information will be filled */
-#define N1SDP_PLATFORM_INFO_BASE		0x06008000
-
 #endif /* N1SDP_DEF_H */
diff --git a/plat/arm/board/n1sdp/n1sdp_err.c b/plat/arm/board/n1sdp/n1sdp_err.c
new file mode 100644
index 0000000..629e76a
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_err.c
@@ -0,0 +1,17 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * n1sdp error handler
+ */
+void __dead2 plat_arm_error_handler(int err)
+{
+	while (true) {
+		wfi();
+	}
+}
diff --git a/plat/arm/board/n1sdp/n1sdp_image_load.c b/plat/arm/board/n1sdp/n1sdp_image_load.c
new file mode 100644
index 0000000..6c3528c
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_image_load.c
@@ -0,0 +1,143 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/desc_image_load.h>
+#include <drivers/arm/css/sds.h>
+#include <libfdt.h>
+#include <plat/common/platform.h>
+
+#include "n1sdp_def.h"
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * Platform information structure stored in SDS.
+ * This structure holds information about platform's DDR
+ * size which will be used to zero out the memory before
+ * enabling the ECC capability as well as information
+ * about multichip setup
+ * 	- multichip mode
+ * 	- secondary_count
+ * 	- Local DDR size in GB, DDR memory in master board
+ * 	- Remote DDR size in GB, DDR memory in secondary board
+ */
+struct n1sdp_plat_info {
+	bool multichip_mode;
+	uint8_t secondary_count;
+	uint8_t local_ddr_size;
+	uint8_t remote_ddr_size;
+} __packed;
+
+/*******************************************************************************
+ * This function inserts Platform information via device tree nodes as,
+ *	platform-info {
+ *		multichip-mode = <0x0>;
+ *		secondary-chip-count = <0x0>;
+ *		local-ddr-size = <0x0>;
+ *		remote-ddr-size = <0x0>;
+ *	};
+ ******************************************************************************/
+static int plat_n1sdp_append_config_node(struct n1sdp_plat_info *plat_info)
+{
+	bl_mem_params_node_t *mem_params;
+	void *fdt;
+	int nodeoffset, err;
+
+	mem_params = get_bl_mem_params_node(NT_FW_CONFIG_ID);
+	if (mem_params == NULL) {
+		ERROR("NT_FW CONFIG base address is NULL\n");
+		return -1;
+	}
+
+	fdt = (void *)(mem_params->image_info.image_base);
+
+	/* Check the validity of the fdt */
+	if (fdt_check_header(fdt) != 0) {
+		ERROR("Invalid NT_FW_CONFIG DTB passed\n");
+		return -1;
+	}
+
+	nodeoffset = fdt_subnode_offset(fdt, 0, "platform-info");
+	if (nodeoffset < 0) {
+		ERROR("NT_FW_CONFIG: Failed to get platform-info node offset\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "multichip-mode",
+			plat_info->multichip_mode);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set multichip-mode\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "secondary-chip-count",
+			plat_info->secondary_count);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set secondary-chip-count\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "local-ddr-size",
+			plat_info->local_ddr_size);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set local-ddr-size\n");
+		return -1;
+	}
+
+	err = fdt_setprop_u32(fdt, nodeoffset, "remote-ddr-size",
+			plat_info->remote_ddr_size);
+	if (err < 0) {
+		ERROR("NT_FW_CONFIG: Failed to set remote-ddr-size\n");
+		return -1;
+	}
+
+	flush_dcache_range((uintptr_t)fdt, mem_params->image_info.image_size);
+
+	return 0;
+}
+
+/*******************************************************************************
+ * This function returns the list of executable images.
+ ******************************************************************************/
+bl_params_t *plat_get_next_bl_params(void)
+{
+	int ret;
+	struct n1sdp_plat_info plat_info;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SDS initialization failed. ret:%d\n", ret);
+		panic();
+	}
+
+	ret = sds_struct_read(N1SDP_SDS_PLATFORM_INFO_STRUCT_ID,
+				N1SDP_SDS_PLATFORM_INFO_OFFSET,
+				&plat_info,
+				N1SDP_SDS_PLATFORM_INFO_SIZE,
+				SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK) {
+		ERROR("Error getting platform info from SDS. ret:%d\n", ret);
+		panic();
+	}
+
+	/* Validate plat_info SDS */
+	if ((plat_info.local_ddr_size == 0U)
+		|| (plat_info.local_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.remote_ddr_size > N1SDP_MAX_DDR_CAPACITY_GB)
+		|| (plat_info.secondary_count > N1SDP_MAX_SECONDARY_COUNT)
+		){
+		ERROR("platform info SDS is corrupted\n");
+		panic();
+	}
+
+	ret = plat_n1sdp_append_config_node(&plat_info);
+	if (ret != 0) {
+		panic();
+	}
+
+	return arm_get_next_bl_params();
+}
diff --git a/plat/arm/board/n1sdp/n1sdp_plat.c b/plat/arm/board/n1sdp/n1sdp_plat.c
index 951a562..502268c 100644
--- a/plat/arm/board/n1sdp/n1sdp_plat.c
+++ b/plat/arm/board/n1sdp/n1sdp_plat.c
@@ -1,16 +1,13 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2022, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <platform_def.h>
+#include <assert.h>
 
-#include <common/bl_common.h>
-#include <common/debug.h>
-#include <plat/arm/common/plat_arm.h>
-#include <plat/common/platform.h>
 #include <drivers/arm/sbsa.h>
+#include <plat/arm/common/plat_arm.h>
 
 #include "n1sdp_def.h"
 
@@ -19,17 +16,51 @@
  * Replace or extend the below regions as required
  */
 
+#if IMAGE_BL1
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	N1SDP_MAP_DEVICE,
+	N1SDP_MAP_NS_SRAM,
+	ARM_MAP_DRAM1,
+	{0}
+};
+#endif
+
+#if IMAGE_BL2
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
 	N1SDP_MAP_DEVICE,
 	N1SDP_MAP_NS_SRAM,
 	ARM_MAP_DRAM1,
 	ARM_MAP_DRAM2,
+#if TRUSTED_BOARD_BOOT && !BL2_AT_EL3
+	ARM_MAP_BL1_RW,
+#endif
+	{0}
+};
+#endif
+
+#if IMAGE_BL31
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	N1SDP_MAP_DEVICE,
+	N1SDP_MAP_NS_SRAM,
 	N1SDP_MAP_REMOTE_DEVICE,
 	N1SDP_MAP_REMOTE_DRAM1,
 	N1SDP_MAP_REMOTE_DRAM2,
 	{0}
 };
+#endif
+
+#if TRUSTED_BOARD_BOOT
+int plat_get_mbedtls_heap(void **heap_addr, size_t *heap_size)
+{
+	assert(heap_addr != NULL);
+	assert(heap_size != NULL);
+
+	return arm_get_mbedtls_heap(heap_addr, heap_size);
+}
+#endif
 
 void plat_arm_secure_wdt_start(void)
 {
diff --git a/plat/arm/board/n1sdp/n1sdp_trusted_boot.c b/plat/arm/board/n1sdp/n1sdp_trusted_boot.c
new file mode 100644
index 0000000..c7dc47f
--- /dev/null
+++ b/plat/arm/board/n1sdp/n1sdp_trusted_boot.c
@@ -0,0 +1,54 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * Return the non-volatile counter value stored in the platform. The cookie
+ * will contain the OID of the counter in the certificate.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+	*nv_ctr = N1SDP_FW_NVCTR_VAL;
+	return 0;
+}
+
+/*
+ * Store a new non-volatile counter value. By default on ARM development
+ * platforms, the non-volatile counters are RO and cannot be modified. We expect
+ * the values in the certificates to always match the RO values so that this
+ * function is never called.
+ *
+ * Return: 0 = success, Otherwise = error
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+	return 1;
+}
+
+/*
+ * Return the ROTPK hash in the following ASN.1 structure in DER format:
+ *
+ * AlgorithmIdentifier  ::=  SEQUENCE  {
+ *     algorithm         OBJECT IDENTIFIER,
+ *     parameters        ANY DEFINED BY algorithm OPTIONAL
+ * }
+ *
+ * DigestInfo ::= SEQUENCE {
+ *     digestAlgorithm   AlgorithmIdentifier,
+ *     digest            OCTET STRING
+ * }
+ */
+int plat_get_rotpk_info(void *cookie, void **key_ptr, unsigned int *key_len,
+			 unsigned int *flags)
+{
+	return arm_get_rotpk_info(cookie, key_ptr, key_len, flags);
+}
+
diff --git a/plat/arm/board/n1sdp/platform.mk b/plat/arm/board/n1sdp/platform.mk
index f20397a..740fb29 100644
--- a/plat/arm/board/n1sdp/platform.mk
+++ b/plat/arm/board/n1sdp/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2018-2022, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -28,30 +28,59 @@
 PLAT_BL_COMMON_SOURCES	:=	${N1SDP_BASE}/n1sdp_plat.c	        \
 				${N1SDP_BASE}/aarch64/n1sdp_helper.S
 
-BL1_SOURCES		+=	drivers/arm/sbsa/sbsa.c
+BL1_SOURCES		:=	${N1SDP_CPU_SOURCES}                \
+				${INTERCONNECT_SOURCES}             \
+				${N1SDP_BASE}/n1sdp_err.c           \
+				${N1SDP_BASE}/n1sdp_trusted_boot.c  \
+				${N1SDP_BASE}/n1sdp_bl1_setup.c     \
+				drivers/arm/sbsa/sbsa.c
+
+BL2_SOURCES		:=	${N1SDP_BASE}/n1sdp_security.c      \
+				${N1SDP_BASE}/n1sdp_err.c           \
+				${N1SDP_BASE}/n1sdp_trusted_boot.c  \
+				lib/utils/mem_region.c              \
+				${N1SDP_BASE}/n1sdp_bl2_setup.c     \
+				${N1SDP_BASE}/n1sdp_image_load.c     \
+				drivers/arm/css/sds/sds.c
 
 BL31_SOURCES		:=	${N1SDP_CPU_SOURCES}			\
 				${INTERCONNECT_SOURCES}			\
 				${N1SDP_GIC_SOURCES}			\
-				${N1SDP_BASE}/n1sdp_bl31_setup.c	        \
+				${N1SDP_BASE}/n1sdp_bl31_setup.c	\
 				${N1SDP_BASE}/n1sdp_topology.c	        \
 				${N1SDP_BASE}/n1sdp_security.c		\
 				drivers/arm/css/sds/sds.c
 
 FDT_SOURCES		+=	fdts/${PLAT}-single-chip.dts	\
-				fdts/${PLAT}-multi-chip.dts
+				fdts/${PLAT}-multi-chip.dts	\
+				${N1SDP_BASE}/fdts/n1sdp_fw_config.dts	\
+				${N1SDP_BASE}/fdts/n1sdp_tb_fw_config.dts	\
+				${N1SDP_BASE}/fdts/n1sdp_nt_fw_config.dts
+
+FW_CONFIG		:=	${BUILD_PLAT}/fdts/n1sdp_fw_config.dtb
+TB_FW_CONFIG		:=	${BUILD_PLAT}/fdts/n1sdp_tb_fw_config.dtb
+NT_FW_CONFIG		:=	${BUILD_PLAT}/fdts/n1sdp_nt_fw_config.dtb
+
+# Add the FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${FW_CONFIG},--fw-config,${FW_CONFIG}))
+# Add the TB_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${TB_FW_CONFIG},--tb-fw-config,${TB_FW_CONFIG}))
+# Add the NT_FW_CONFIG to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${NT_FW_CONFIG},--nt-fw-config,${NT_FW_CONFIG}))
+
+# Setting to 0 as no NVCTR in N1SDP
+N1SDP_FW_NVCTR_VAL	:=	0
+TFW_NVCTR_VAL		:=	${N1SDP_FW_NVCTR_VAL}
+NTFW_NVCTR_VAL		:=	${N1SDP_FW_NVCTR_VAL}
+
+# Add N1SDP_FW_NVCTR_VAL
+$(eval $(call add_define,N1SDP_FW_NVCTR_VAL))
 
 # TF-A not required to load the SCP Images
 override CSS_LOAD_SCP_IMAGES	  	:=	0
 
-# BL1/BL2 Image not a part of the capsule Image for n1sdp
-override NEED_BL1		  	:=	no
-override NEED_BL2		  	:=	no
 override NEED_BL2U		  	:=	no
 
-#TFA for n1sdp starts from BL31
-override RESET_TO_BL31            	:=	1
-
 # 32 bit mode not supported
 override CTX_INCLUDE_AARCH32_REGS 	:=	0
 
@@ -73,4 +102,3 @@
 include plat/arm/common/arm_common.mk
 include plat/arm/css/common/css_common.mk
 include plat/arm/board/common/board_common.mk
-
diff --git a/plat/hisilicon/hikey/platform.mk b/plat/hisilicon/hikey/platform.mk
index 18197cf..3e1771c 100644
--- a/plat/hisilicon/hikey/platform.mk
+++ b/plat/hisilicon/hikey/platform.mk
@@ -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
 #
@@ -95,6 +95,10 @@
 BL2_SOURCES		+=	lib/optee/optee_utils.c
 endif
 
+include lib/zlib/zlib.mk
+PLAT_INCLUDES		+=	-Ilib/zlib
+BL2_SOURCES		+=	$(ZLIB_SOURCES)
+
 HIKEY_GIC_SOURCES	:=	drivers/arm/gic/common/gic_common.c	\
 				drivers/arm/gic/v2/gicv2_main.c		\
 				drivers/arm/gic/v2/gicv2_helpers.c	\
diff --git a/plat/hisilicon/hikey960/platform.mk b/plat/hisilicon/hikey960/platform.mk
index fc2c209..608fe09 100644
--- a/plat/hisilicon/hikey960/platform.mk
+++ b/plat/hisilicon/hikey960/platform.mk
@@ -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
 #
@@ -94,6 +94,10 @@
 BL2_SOURCES		+=	lib/optee/optee_utils.c
 endif
 
+include lib/zlib/zlib.mk
+PLAT_INCLUDES		+=	-Ilib/zlib
+BL2_SOURCES		+=	$(ZLIB_SOURCES)
+
 BL31_SOURCES		+=	drivers/arm/cci/cci.c			\
 				drivers/arm/pl061/pl061_gpio.c		\
 				drivers/gpio/gpio.c			\
diff --git a/plat/intel/soc/agilex/platform.mk b/plat/intel/soc/agilex/platform.mk
index 6fe0be1..ccb4e07 100644
--- a/plat/intel/soc/agilex/platform.mk
+++ b/plat/intel/soc/agilex/platform.mk
@@ -56,6 +56,10 @@
 		plat/intel/soc/common/drivers/qspi/cadence_qspi.c	\
 		plat/intel/soc/common/drivers/wdt/watchdog.c
 
+include lib/zlib/zlib.mk
+PLAT_INCLUDES	+=	-Ilib/zlib
+BL2_SOURCES	+=	$(ZLIB_SOURCES)
+
 BL31_SOURCES	+=	\
 		drivers/arm/cci/cci.c					\
 		lib/cpus/aarch64/aem_generic.S				\
diff --git a/plat/intel/soc/stratix10/platform.mk b/plat/intel/soc/stratix10/platform.mk
index 8b39b6f..5c0b421 100644
--- a/plat/intel/soc/stratix10/platform.mk
+++ b/plat/intel/soc/stratix10/platform.mk
@@ -55,6 +55,10 @@
 		plat/intel/soc/common/drivers/qspi/cadence_qspi.c	\
 		plat/intel/soc/common/drivers/wdt/watchdog.c
 
+include lib/zlib/zlib.mk
+PLAT_INCLUDES	+=	-Ilib/zlib
+BL2_SOURCES	+=	$(ZLIB_SOURCES)
+
 BL31_SOURCES	+=	\
 		drivers/arm/cci/cci.c					\
 		lib/cpus/aarch64/aem_generic.S				\
diff --git a/plat/st/stm32mp1/plat_image_load.c b/plat/st/stm32mp1/plat_image_load.c
index 76af0fc..f68eb38 100644
--- a/plat/st/stm32mp1/plat_image_load.c
+++ b/plat/st/stm32mp1/plat_image_load.c
@@ -4,6 +4,8 @@
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+#include <assert.h>
+
 #include <common/desc_image_load.h>
 #include <plat/common/platform.h>
 
diff --git a/plat/st/stm32mp1/platform.mk b/plat/st/stm32mp1/platform.mk
index 9e67989..127e318 100644
--- a/plat/st/stm32mp1/platform.mk
+++ b/plat/st/stm32mp1/platform.mk
@@ -320,12 +320,14 @@
 				plat/st/stm32mp1/stm32mp1_security.c
 endif
 
-ifeq (${PSA_FWU_SUPPORT},1)
 include lib/zlib/zlib.mk
+
+ifeq (${PSA_FWU_SUPPORT},1)
 include drivers/fwu/fwu.mk
+endif
+
 
 BL2_SOURCES		+=	$(ZLIB_SOURCES)
-endif
 
 BL2_SOURCES		+=	drivers/io/io_block.c					\
 				drivers/io/io_mtd.c					\
diff --git a/plat/xilinx/common/include/ipi.h b/plat/xilinx/common/include/ipi.h
index 483902e..ac76bf0 100644
--- a/plat/xilinx/common/include/ipi.h
+++ b/plat/xilinx/common/include/ipi.h
@@ -47,7 +47,7 @@
  ********************************************************************/
 
 /* Initialize IPI configuration table */
-void ipi_config_table_init(const struct ipi_config *ipi_table,
+void ipi_config_table_init(const struct ipi_config *ipi_config_table,
 			   uint32_t total_ipi);
 
 /* Validate IPI mailbox access */
diff --git a/plat/xilinx/common/include/plat_startup.h b/plat/xilinx/common/include/plat_startup.h
index 66e7933..6799e21 100644
--- a/plat/xilinx/common/include/plat_startup.h
+++ b/plat/xilinx/common/include/plat_startup.h
@@ -15,8 +15,8 @@
 	FSBL_HANDOFF_TOO_MANY_PARTS
 };
 
-enum fsbl_handoff fsbl_atf_handover(entry_point_info_t *bl32_image_ep_info,
-					entry_point_info_t *bl33_image_ep_info,
+enum fsbl_handoff fsbl_atf_handover(entry_point_info_t *bl32,
+					entry_point_info_t *bl33,
 					uint64_t atf_handoff_addr);
 
 #endif /* PLAT_STARTUP_H */
diff --git a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
index 5ad33cc..6ded2e2 100644
--- a/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
+++ b/plat/xilinx/zynqmp/bl31_zynqmp_setup.c
@@ -33,15 +33,18 @@
  * while BL32 corresponds to the secure image type. A NULL pointer is returned
  * if the image does not exist.
  */
-entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type)
+struct entry_point_info *bl31_plat_get_next_image_ep_info(uint32_t type)
 {
-	assert(sec_state_is_valid(type));
+	entry_point_info_t *next_image_info;
 
+	assert(sec_state_is_valid(type));
 	if (type == NON_SECURE) {
-		return &bl33_image_ep_info;
+		next_image_info = &bl33_image_ep_info;
+	} else {
+		next_image_info = &bl32_image_ep_info;
 	}
 
-	return &bl32_image_ep_info;
+	return next_image_info;
 }
 
 /*
diff --git a/plat/xilinx/zynqmp/plat_psci.c b/plat/xilinx/zynqmp/plat_psci.c
index 881dfe6..f337cf5 100644
--- a/plat/xilinx/zynqmp/plat_psci.c
+++ b/plat/xilinx/zynqmp/plat_psci.c
@@ -19,9 +19,9 @@
 #include "pm_api_sys.h"
 #include "pm_client.h"
 
-uintptr_t zynqmp_sec_entry;
+static uintptr_t zynqmp_sec_entry;
 
-void zynqmp_cpu_standby(plat_local_state_t cpu_state)
+static void zynqmp_cpu_standby(plat_local_state_t cpu_state)
 {
 	VERBOSE("%s: cpu_state: 0x%x\n", __func__, cpu_state);
 
@@ -171,7 +171,7 @@
 	}
 }
 
-int zynqmp_validate_power_state(unsigned int power_state,
+static int zynqmp_validate_power_state(unsigned int power_state,
 				psci_power_state_t *req_state)
 {
 	VERBOSE("%s: power_state: 0x%x\n", __func__, power_state);
@@ -194,7 +194,7 @@
 	return PSCI_E_SUCCESS;
 }
 
-void zynqmp_get_sys_suspend_power_state(psci_power_state_t *req_state)
+static void zynqmp_get_sys_suspend_power_state(psci_power_state_t *req_state)
 {
 	req_state->pwr_domain_state[PSCI_CPU_PWR_LVL] = PLAT_MAX_OFF_STATE;
 	req_state->pwr_domain_state[1] = PLAT_MAX_OFF_STATE;
diff --git a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
index 48b3877..84b239c 100644
--- a/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
+++ b/plat/xilinx/zynqmp/pm_service/pm_api_sys.h
@@ -68,7 +68,7 @@
 /**********************************************************
  * System-level API function declarations
  **********************************************************/
-enum pm_ret_status pm_req_suspend(enum pm_node_id nid,
+enum pm_ret_status pm_req_suspend(enum pm_node_id target,
 				  enum pm_request_ack ack,
 				  unsigned int latency,
 				  unsigned int state);
@@ -78,12 +78,12 @@
 				   unsigned int state,
 				   uintptr_t address);
 
-enum pm_ret_status pm_force_powerdown(enum pm_node_id nid,
+enum pm_ret_status pm_force_powerdown(enum pm_node_id target,
 				      enum pm_request_ack ack);
 
 enum pm_ret_status pm_abort_suspend(enum pm_abort_reason reason);
 
-enum pm_ret_status pm_req_wakeup(enum pm_node_id nid,
+enum pm_ret_status pm_req_wakeup(enum pm_node_id target,
 				 unsigned int set_address,
 				 uintptr_t address,
 				 enum pm_request_ack ack);
@@ -112,7 +112,7 @@
 
 /* Miscellaneous API functions */
 enum pm_ret_status pm_get_api_version(unsigned int *version);
-enum pm_ret_status pm_get_node_status(enum pm_node_id node,
+enum pm_ret_status pm_get_node_status(enum pm_node_id nid,
 				      uint32_t *ret_buff);
 enum pm_ret_status pm_acknowledge_cb(enum pm_node_id nid,
 				     enum pm_ret_status status,
@@ -133,8 +133,8 @@
 enum pm_ret_status pm_fpga_get_status(unsigned int *value);
 
 enum pm_ret_status pm_get_chipid(uint32_t *value);
-enum pm_ret_status pm_secure_rsaaes(uint32_t address_high,
-				    uint32_t address_low,
+enum pm_ret_status pm_secure_rsaaes(uint32_t address_low,
+				    uint32_t address_high,
 				    uint32_t size,
 				    uint32_t flags);
 unsigned int pm_get_shutdown_scope(void);
@@ -157,9 +157,9 @@
 enum pm_ret_status pm_clock_getrate(unsigned int clock_id,
 				    uint64_t *rate);
 enum pm_ret_status pm_clock_setparent(unsigned int clock_id,
-				      unsigned int parent_id);
+				      unsigned int parent_index);
 enum pm_ret_status pm_clock_getparent(unsigned int clock_id,
-				      unsigned int *parent_id);
+				      unsigned int *parent_index);
 void pm_query_data(enum pm_query_id qid, unsigned int arg1, unsigned int arg2,
 		   unsigned int arg3, unsigned int *data);
 enum pm_ret_status pm_sha_hash(uint32_t address_high,
diff --git a/plat/xilinx/zynqmp/sip_svc_setup.c b/plat/xilinx/zynqmp/sip_svc_setup.c
index 4a6095c..4ce9b8a 100644
--- a/plat/xilinx/zynqmp/sip_svc_setup.c
+++ b/plat/xilinx/zynqmp/sip_svc_setup.c
@@ -53,7 +53,7 @@
  * Handler for all SiP SMC calls. Handles standard SIP requests
  * and calls PM SMC handler if the call is for a PM-API function.
  */
-uintptr_t sip_svc_smc_handler(uint32_t smc_fid,
+static uintptr_t sip_svc_smc_handler(uint32_t smc_fid,
 			      u_register_t x1,
 			      u_register_t x2,
 			      u_register_t x3,
diff --git a/services/std_svc/spm/el3_spmc/spmc.h b/services/std_svc/spm/el3_spmc/spmc.h
index faa604f..d62be91 100644
--- a/services/std_svc/spm/el3_spmc/spmc.h
+++ b/services/std_svc/spm/el3_spmc/spmc.h
@@ -33,11 +33,31 @@
 /* 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
+/* FF-A Related helper macros. */
+#define FFA_ID_MASK			U(0xFFFF)
+#define FFA_PARTITION_ID_SHIFT		U(16)
+#define FFA_FEATURES_BIT31_MASK		U(0x1u << 31)
+#define FFA_FEATURES_RET_REQ_NS_BIT	U(0x1 << 1)
+
+#define FFA_RUN_EP_ID(ep_vcpu_ids) \
+		((ep_vcpu_ids >> FFA_PARTITION_ID_SHIFT) & FFA_ID_MASK)
+#define FFA_RUN_VCPU_ID(ep_vcpu_ids) \
+		(ep_vcpu_ids & FFA_ID_MASK)
+
+#define FFA_PAGE_SIZE (4096)
+#define FFA_RXTX_PAGE_COUNT_MASK 0x1F
+
+/* Ensure that the page size used by TF-A is 4k aligned. */
+CASSERT((PAGE_SIZE % FFA_PAGE_SIZE) == 0, assert_aligned_page_size);
 
 /*
+ * Defines to allow an SP to subscribe for power management messages
+ */
+#define FFA_PM_MSG_SUB_CPU_OFF			U(1 << 0)
+#define FFA_PM_MSG_SUB_CPU_SUSPEND		U(1 << 1)
+#define FFA_PM_MSG_SUB_CPU_SUSPEND_RESUME	U(1 << 2)
+
+/*
  * Runtime states of an execution context as per the FF-A v1.1 specification.
  */
 enum sp_runtime_states {
@@ -146,6 +166,17 @@
 
 	/* Secondary entrypoint. Only valid for a S-EL1 SP. */
 	uintptr_t secondary_ep;
+
+	/*
+	 * Store whether the SP has subscribed to any power management messages.
+	 */
+	uint16_t pwr_mgmt_msgs;
+
+	/*
+	 * Store whether the SP has requested the use of the NS bit for memory
+	 * management transactions if it is using FF-A v1.0.
+	 */
+	bool ns_bit_requested;
 };
 
 /*
@@ -178,9 +209,31 @@
 	uint32_t ffa_version;
 };
 
+/**
+ * Holds information returned for each partition by the FFA_PARTITION_INFO_GET
+ * interface.
+ */
+struct ffa_partition_info_v1_0 {
+	uint16_t ep_id;
+	uint16_t execution_ctx_count;
+	uint32_t properties;
+};
+
+/* Extended structure for v1.1. */
+struct ffa_partition_info_v1_1 {
+	uint16_t ep_id;
+	uint16_t execution_ctx_count;
+	uint32_t properties;
+	uint32_t uuid[4];
+};
+
+/* Reference to power management hooks */
+extern const spd_pm_ops_t spmc_pm;
+
 /* Setup Function for different SP types. */
 void spmc_sp_common_setup(struct secure_partition_desc *sp,
-			  entry_point_info_t *ep_info);
+			  entry_point_info_t *ep_info,
+			  int32_t boot_info_reg);
 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,
@@ -227,4 +280,16 @@
  */
 struct mailbox *spmc_get_mbox_desc(bool secure_origin);
 
+/*
+ * Helper function to obtain the context of an SP with a given partition ID.
+ */
+struct secure_partition_desc *spmc_get_sp_ctx(uint16_t id);
+
+/*
+ * Add helper function to obtain the FF-A version of the calling
+ * partition.
+ */
+uint32_t get_partition_ffa_version(bool secure_origin);
+
+
 #endif /* SPMC_H */
diff --git a/services/std_svc/spm/el3_spmc/spmc.mk b/services/std_svc/spm/el3_spmc/spmc.mk
index 8067c74..aa591d9 100644
--- a/services/std_svc/spm/el3_spmc/spmc.mk
+++ b/services/std_svc/spm/el3_spmc/spmc.mk
@@ -11,7 +11,9 @@
 SPMC_SOURCES	:=	$(addprefix services/std_svc/spm/el3_spmc/,	\
 			spmc_main.c				\
 			spmc_setup.c				\
-			logical_sp.c)
+			logical_sp.c				\
+			spmc_pm.c				\
+			spmc_shared_mem.c)
 
 # Specify platform specific logical partition implementation.
 SPMC_LP_SOURCES  := $(addprefix ${PLAT_DIR}/, \
diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
index 33a25a2..9b8621a 100644
--- a/services/std_svc/spm/el3_spmc/spmc_main.c
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c
@@ -10,9 +10,11 @@
 #include <arch_helpers.h>
 #include <bl31/bl31.h>
 #include <bl31/ehf.h>
+#include <bl31/interrupt_mgmt.h>
 #include <common/debug.h>
 #include <common/fdt_wrappers.h>
 #include <common/runtime_svc.h>
+#include <common/uuid.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/smccc.h>
 #include <lib/utils.h>
@@ -24,9 +26,13 @@
 #include <services/spmc_svc.h>
 #include <services/spmd_svc.h>
 #include "spmc.h"
+#include "spmc_shared_mem.h"
 
 #include <platform_def.h>
 
+/* Declare the maximum number of SPs and El3 LPs. */
+#define MAX_SP_LP_PARTITIONS SECURE_PARTITION_COUNT + MAX_EL3_LP_DESCS_COUNT
+
 /*
  * Allocate a secure partition descriptor to describe each SP in the system that
  * does not reside at EL3.
@@ -41,6 +47,11 @@
  */
 static struct ns_endpoint_desc ns_ep_desc[NS_PARTITION_COUNT];
 
+static uint64_t spmc_sp_interrupt_handler(uint32_t id,
+					  uint32_t flags,
+					  void *handle,
+					  void *cookie);
+
 /*
  * Helper function to obtain the array storing the EL3
  * Logical Partition descriptors.
@@ -230,13 +241,20 @@
  ******************************************************************************/
 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;
+	/* Check message type. */
+	if (x2 & FFA_FWK_MSG_BIT) {
+		/* We have a framework message, ensure it is a known message. */
+		if (x2 & ~(FFA_FWK_MSG_MASK | FFA_FWK_MSG_BIT)) {
+			VERBOSE("Invalid message format 0x%lx.\n", x2);
+			return false;
+		}
+	} else {
+		/* We have a partition messages, ensure x2 is not set. */
+		if (x2 != (uint64_t) 0) {
+			VERBOSE("Arg2 MBZ for partition messages. (0x%lx).\n",
+				x2);
+			return false;
+		}
 	}
 	return true;
 }
@@ -565,6 +583,808 @@
 	} else {
 		return spmc_get_hyp_ctx()->ffa_version;
 	}
+}
+
+static uint64_t rxtx_map_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)
+{
+	int ret;
+	uint32_t error_code;
+	uint32_t mem_atts = secure_origin ? MT_SECURE : MT_NS;
+	struct mailbox *mbox;
+	uintptr_t tx_address = x1;
+	uintptr_t rx_address = x2;
+	uint32_t page_count = x3 & FFA_RXTX_PAGE_COUNT_MASK; /* Bits [5:0] */
+	uint32_t buf_size = page_count * FFA_PAGE_SIZE;
+
+	/*
+	 * The SPMC does not support mapping of VM RX/TX pairs to facilitate
+	 * indirect messaging with SPs. Check if the Hypervisor has invoked this
+	 * ABI on behalf of a VM and reject it if this is the case.
+	 */
+	if (tx_address == 0 || rx_address == 0) {
+		WARN("Mapping RX/TX Buffers on behalf of VM not supported.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Ensure the specified buffers are not the same. */
+	if (tx_address == rx_address) {
+		WARN("TX Buffer must not be the same as RX Buffer.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Ensure the buffer size is not 0. */
+	if (buf_size == 0U) {
+		WARN("Buffer size must not be 0\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Ensure the buffer size is a multiple of the translation granule size
+	 * in TF-A.
+	 */
+	if (buf_size % PAGE_SIZE != 0U) {
+		WARN("Buffer size must be aligned to translation granule.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Obtain the RX/TX buffer pair descriptor. */
+	mbox = spmc_get_mbox_desc(secure_origin);
+
+	spin_lock(&mbox->lock);
+
+	/* Check if buffers have already been mapped. */
+	if (mbox->rx_buffer != 0 || mbox->tx_buffer != 0) {
+		WARN("RX/TX Buffers already mapped (%p/%p)\n",
+		     (void *) mbox->rx_buffer, (void *)mbox->tx_buffer);
+		error_code = FFA_ERROR_DENIED;
+		goto err;
+	}
+
+	/* memmap the TX buffer as read only. */
+	ret = mmap_add_dynamic_region(tx_address, /* PA */
+			tx_address, /* VA */
+			buf_size, /* size */
+			mem_atts | MT_RO_DATA); /* attrs */
+	if (ret != 0) {
+		/* Return the correct error code. */
+		error_code = (ret == -ENOMEM) ? FFA_ERROR_NO_MEMORY :
+						FFA_ERROR_INVALID_PARAMETER;
+		WARN("Unable to map TX buffer: %d\n", error_code);
+		goto err;
+	}
+
+	/* memmap the RX buffer as read write. */
+	ret = mmap_add_dynamic_region(rx_address, /* PA */
+			rx_address, /* VA */
+			buf_size, /* size */
+			mem_atts | MT_RW_DATA); /* attrs */
+
+	if (ret != 0) {
+		error_code = (ret == -ENOMEM) ? FFA_ERROR_NO_MEMORY :
+						FFA_ERROR_INVALID_PARAMETER;
+		WARN("Unable to map RX buffer: %d\n", error_code);
+		/* Unmap the TX buffer again. */
+		mmap_remove_dynamic_region(tx_address, buf_size);
+		goto err;
+	}
+
+	mbox->tx_buffer = (void *) tx_address;
+	mbox->rx_buffer = (void *) rx_address;
+	mbox->rxtx_page_count = page_count;
+	spin_unlock(&mbox->lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+	/* Execution stops here. */
+err:
+	spin_unlock(&mbox->lock);
+	return spmc_ffa_error_return(handle, error_code);
+}
+
+static uint64_t rxtx_unmap_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 mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	uint32_t buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+
+	/*
+	 * The SPMC does not support mapping of VM RX/TX pairs to facilitate
+	 * indirect messaging with SPs. Check if the Hypervisor has invoked this
+	 * ABI on behalf of a VM and reject it if this is the case.
+	 */
+	if (x1 != 0UL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&mbox->lock);
+
+	/* Check if buffers are currently mapped. */
+	if (mbox->rx_buffer == 0 || mbox->tx_buffer == 0) {
+		spin_unlock(&mbox->lock);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Unmap RX Buffer */
+	if (mmap_remove_dynamic_region((uintptr_t) mbox->rx_buffer,
+				       buf_size) != 0) {
+		WARN("Unable to unmap RX buffer!\n");
+	}
+
+	mbox->rx_buffer = 0;
+
+	/* Unmap TX Buffer */
+	if (mmap_remove_dynamic_region((uintptr_t) mbox->tx_buffer,
+				       buf_size) != 0) {
+		WARN("Unable to unmap TX buffer!\n");
+	}
+
+	mbox->tx_buffer = 0;
+	mbox->rxtx_page_count = 0;
+
+	spin_unlock(&mbox->lock);
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+}
+
+/*
+ * Collate the partition information in a v1.1 partition information
+ * descriptor format, this will be converter later if required.
+ */
+static int partition_info_get_handler_v1_1(uint32_t *uuid,
+					   struct ffa_partition_info_v1_1
+						  *partitions,
+					   uint32_t max_partitions,
+					   uint32_t *partition_count)
+{
+	uint32_t index;
+	struct ffa_partition_info_v1_1 *desc;
+	bool null_uuid = is_null_uuid(uuid);
+	struct el3_lp_desc *el3_lp_descs = get_el3_lp_array();
+
+	/* Deal with Logical Partitions. */
+	for (index = 0U; index < EL3_LP_DESCS_COUNT; index++) {
+		if (null_uuid || uuid_match(uuid, el3_lp_descs[index].uuid)) {
+			/* Found a matching UUID, populate appropriately. */
+			if (*partition_count >= max_partitions) {
+				return FFA_ERROR_NO_MEMORY;
+			}
+
+			desc = &partitions[*partition_count];
+			desc->ep_id = el3_lp_descs[index].sp_id;
+			desc->execution_ctx_count = PLATFORM_CORE_COUNT;
+			desc->properties = el3_lp_descs[index].properties;
+			if (null_uuid) {
+				copy_uuid(desc->uuid, el3_lp_descs[index].uuid);
+			}
+			(*partition_count)++;
+		}
+	}
+
+	/* Deal with physical SP's. */
+	for (index = 0U; index < SECURE_PARTITION_COUNT; index++) {
+		if (null_uuid || uuid_match(uuid, sp_desc[index].uuid)) {
+			/* Found a matching UUID, populate appropriately. */
+			if (*partition_count >= max_partitions) {
+				return FFA_ERROR_NO_MEMORY;
+			}
+
+			desc = &partitions[*partition_count];
+			desc->ep_id = sp_desc[index].sp_id;
+			/*
+			 * Execution context count must match No. cores for
+			 * S-EL1 SPs.
+			 */
+			desc->execution_ctx_count = PLATFORM_CORE_COUNT;
+			desc->properties = sp_desc[index].properties;
+			if (null_uuid) {
+				copy_uuid(desc->uuid, sp_desc[index].uuid);
+			}
+			(*partition_count)++;
+		}
+	}
+	return 0;
+}
+
+/*
+ * Handle the case where that caller only wants the count of partitions
+ * matching a given UUID and does not want the corresponding descriptors
+ * populated.
+ */
+static uint32_t partition_info_get_handler_count_only(uint32_t *uuid)
+{
+	uint32_t index = 0;
+	uint32_t partition_count = 0;
+	bool null_uuid = is_null_uuid(uuid);
+	struct el3_lp_desc *el3_lp_descs = get_el3_lp_array();
+
+	/* Deal with Logical Partitions. */
+	for (index = 0U; index < EL3_LP_DESCS_COUNT; index++) {
+		if (null_uuid ||
+		    uuid_match(uuid, el3_lp_descs[index].uuid)) {
+			(partition_count)++;
+		}
+	}
+
+	/* Deal with physical SP's. */
+	for (index = 0U; index < SECURE_PARTITION_COUNT; index++) {
+		if (null_uuid || uuid_match(uuid, sp_desc[index].uuid)) {
+			(partition_count)++;
+		}
+	}
+	return partition_count;
+}
+
+/*
+ * If the caller of the PARTITION_INFO_GET ABI was a v1.0 caller, populate
+ * the coresponding descriptor format from the v1.1 descriptor array.
+ */
+static uint64_t partition_info_populate_v1_0(struct ffa_partition_info_v1_1
+					     *partitions,
+					     struct mailbox *mbox,
+					     int partition_count)
+{
+	uint32_t index;
+	uint32_t buf_size;
+	uint32_t descriptor_size;
+	struct ffa_partition_info_v1_0 *v1_0_partitions =
+		(struct ffa_partition_info_v1_0 *) mbox->rx_buffer;
+
+	buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+	descriptor_size = partition_count *
+			  sizeof(struct ffa_partition_info_v1_0);
+
+	if (descriptor_size > buf_size) {
+		return FFA_ERROR_NO_MEMORY;
+	}
+
+	for (index = 0U; index < partition_count; index++) {
+		v1_0_partitions[index].ep_id = partitions[index].ep_id;
+		v1_0_partitions[index].execution_ctx_count =
+			partitions[index].execution_ctx_count;
+		v1_0_partitions[index].properties =
+			partitions[index].properties;
+	}
+	return 0;
+}
+
+/*
+ * Main handler for FFA_PARTITION_INFO_GET which supports both FF-A v1.1 and
+ * v1.0 implementations.
+ */
+static uint64_t partition_info_get_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)
+{
+	int ret;
+	uint32_t partition_count = 0;
+	uint32_t size = 0;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+	struct mailbox *mbox;
+	uint64_t info_get_flags;
+	bool count_only;
+	uint32_t uuid[4];
+
+	uuid[0] = x1;
+	uuid[1] = x2;
+	uuid[2] = x3;
+	uuid[3] = x4;
+
+	/* Determine if the Partition descriptors should be populated. */
+	info_get_flags = SMC_GET_GP(handle, CTX_GPREG_X5);
+	count_only = (info_get_flags & FFA_PARTITION_INFO_GET_COUNT_FLAG_MASK);
+
+	/* Handle the case where we don't need to populate the descriptors. */
+	if (count_only) {
+		partition_count = partition_info_get_handler_count_only(uuid);
+		if (partition_count == 0) {
+			return spmc_ffa_error_return(handle,
+						FFA_ERROR_INVALID_PARAMETER);
+		}
+	} else {
+		struct ffa_partition_info_v1_1 partitions[MAX_SP_LP_PARTITIONS];
+
+		/*
+		 * Handle the case where the partition descriptors are required,
+		 * check we have the buffers available and populate the
+		 * appropriate structure version.
+		 */
+
+		/* Obtain the v1.1 format of the descriptors. */
+		ret = partition_info_get_handler_v1_1(uuid, partitions,
+						      MAX_SP_LP_PARTITIONS,
+						      &partition_count);
+
+		/* Check if an error occurred during discovery. */
+		if (ret != 0) {
+			goto err;
+		}
+
+		/* If we didn't find any matches the UUID is unknown. */
+		if (partition_count == 0) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err;
+		}
+
+		/* Obtain the partition mailbox RX/TX buffer pair descriptor. */
+		mbox = spmc_get_mbox_desc(secure_origin);
+
+		/*
+		 * If the caller has not bothered registering its RX/TX pair
+		 * then return an error code.
+		 */
+		spin_lock(&mbox->lock);
+		if (mbox->rx_buffer == NULL) {
+			ret = FFA_ERROR_BUSY;
+			goto err_unlock;
+		}
+
+		/* Ensure the RX buffer is currently free. */
+		if (mbox->state != MAILBOX_STATE_EMPTY) {
+			ret = FFA_ERROR_BUSY;
+			goto err_unlock;
+		}
+
+		/* Zero the RX buffer before populating. */
+		(void)memset(mbox->rx_buffer, 0,
+			     mbox->rxtx_page_count * FFA_PAGE_SIZE);
+
+		/*
+		 * Depending on the FF-A version of the requesting partition
+		 * we may need to convert to a v1.0 format otherwise we can copy
+		 * directly.
+		 */
+		if (ffa_version == MAKE_FFA_VERSION(U(1), U(0))) {
+			ret = partition_info_populate_v1_0(partitions,
+							   mbox,
+							   partition_count);
+			if (ret != 0) {
+				goto err_unlock;
+			}
+		} else {
+			uint32_t buf_size = mbox->rxtx_page_count *
+					    FFA_PAGE_SIZE;
+
+			/* Ensure the descriptor will fit in the buffer. */
+			size = sizeof(struct ffa_partition_info_v1_1);
+			if (partition_count * size  > buf_size) {
+				ret = FFA_ERROR_NO_MEMORY;
+				goto err_unlock;
+			}
+			memcpy(mbox->rx_buffer, partitions,
+			       partition_count * size);
+		}
+
+		mbox->state = MAILBOX_STATE_FULL;
+		spin_unlock(&mbox->lock);
+	}
+	SMC_RET4(handle, FFA_SUCCESS_SMC32, 0, partition_count, size);
+
+err_unlock:
+	spin_unlock(&mbox->lock);
+err:
+	return spmc_ffa_error_return(handle, ret);
+}
+
+static uint64_t ffa_feature_success(void *handle, uint32_t arg2)
+{
+	SMC_RET3(handle, FFA_SUCCESS_SMC32, 0, arg2);
+}
+
+static uint64_t ffa_features_retrieve_request(bool secure_origin,
+					      uint32_t input_properties,
+					      void *handle)
+{
+	/*
+	 * If we're called by the normal world we don't support any
+	 * additional features.
+	 */
+	if (!secure_origin) {
+		if ((input_properties & FFA_FEATURES_RET_REQ_NS_BIT) != 0U) {
+			return spmc_ffa_error_return(handle,
+						     FFA_ERROR_NOT_SUPPORTED);
+		}
+
+	} else {
+		struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+		/*
+		 * If v1.1 the NS bit must be set otherwise it is an invalid
+		 * call. If v1.0 check and store whether the SP has requested
+		 * the use of the NS bit.
+		 */
+		if (sp->ffa_version == MAKE_FFA_VERSION(1, 1)) {
+			if ((input_properties &
+			     FFA_FEATURES_RET_REQ_NS_BIT) == 0U) {
+				return spmc_ffa_error_return(handle,
+						       FFA_ERROR_NOT_SUPPORTED);
+			}
+			return ffa_feature_success(handle,
+						   FFA_FEATURES_RET_REQ_NS_BIT);
+		} else {
+			sp->ns_bit_requested = (input_properties &
+					       FFA_FEATURES_RET_REQ_NS_BIT) !=
+					       0U;
+		}
+		if (sp->ns_bit_requested) {
+			return ffa_feature_success(handle,
+						   FFA_FEATURES_RET_REQ_NS_BIT);
+		}
+	}
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+}
+
+static uint64_t ffa_features_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)
+{
+	uint32_t function_id = (uint32_t) x1;
+	uint32_t input_properties = (uint32_t) x2;
+
+	/* Check if a Feature ID was requested. */
+	if ((function_id & FFA_FEATURES_BIT31_MASK) == 0U) {
+		/* We currently don't support any additional features. */
+		return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/*
+	 * Handle the cases where we have separate handlers due to additional
+	 * properties.
+	 */
+	switch (function_id) {
+	case FFA_MEM_RETRIEVE_REQ_SMC32:
+	case FFA_MEM_RETRIEVE_REQ_SMC64:
+		return ffa_features_retrieve_request(secure_origin,
+						     input_properties,
+						     handle);
+	}
+
+	/*
+	 * We don't currently support additional input properties for these
+	 * other ABIs therefore ensure this value is set to 0.
+	 */
+	if (input_properties != 0U) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/* Report if any other FF-A ABI is supported. */
+	switch (function_id) {
+	/* Supported features from both worlds. */
+	case FFA_ERROR:
+	case FFA_SUCCESS_SMC32:
+	case FFA_INTERRUPT:
+	case FFA_SPM_ID_GET:
+	case FFA_ID_GET:
+	case FFA_FEATURES:
+	case FFA_VERSION:
+	case FFA_RX_RELEASE:
+	case FFA_MSG_SEND_DIRECT_REQ_SMC32:
+	case FFA_MSG_SEND_DIRECT_REQ_SMC64:
+	case FFA_PARTITION_INFO_GET:
+	case FFA_RXTX_MAP_SMC32:
+	case FFA_RXTX_MAP_SMC64:
+	case FFA_RXTX_UNMAP:
+	case FFA_MEM_FRAG_TX:
+	case FFA_MSG_RUN:
+
+		/*
+		 * We are relying on the fact that the other registers
+		 * will be set to 0 as these values align with the
+		 * currently implemented features of the SPMC. If this
+		 * changes this function must be extended to handle
+		 * reporting the additional functionality.
+		 */
+
+		SMC_RET1(handle, FFA_SUCCESS_SMC32);
+		/* Execution stops here. */
+
+	/* Supported ABIs only from the secure world. */
+	case FFA_SECONDARY_EP_REGISTER_SMC64:
+	case FFA_MSG_SEND_DIRECT_RESP_SMC32:
+	case FFA_MSG_SEND_DIRECT_RESP_SMC64:
+	case FFA_MEM_RELINQUISH:
+	case FFA_MSG_WAIT:
+
+		if (!secure_origin) {
+			return spmc_ffa_error_return(handle,
+				FFA_ERROR_NOT_SUPPORTED);
+		}
+		SMC_RET1(handle, FFA_SUCCESS_SMC32);
+		/* Execution stops here. */
+
+	/* Supported features only from the normal world. */
+	case FFA_MEM_SHARE_SMC32:
+	case FFA_MEM_SHARE_SMC64:
+	case FFA_MEM_LEND_SMC32:
+	case FFA_MEM_LEND_SMC64:
+	case FFA_MEM_RECLAIM:
+	case FFA_MEM_FRAG_RX:
+
+		if (secure_origin) {
+			return spmc_ffa_error_return(handle,
+					FFA_ERROR_NOT_SUPPORTED);
+		}
+		SMC_RET1(handle, FFA_SUCCESS_SMC32);
+		/* Execution stops here. */
+
+	default:
+		return spmc_ffa_error_return(handle,
+					FFA_ERROR_NOT_SUPPORTED);
+	}
+}
+
+static uint64_t ffa_id_get_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)
+{
+	if (secure_origin) {
+		SMC_RET3(handle, FFA_SUCCESS_SMC32, 0x0,
+			 spmc_get_current_sp_ctx()->sp_id);
+	} else {
+		SMC_RET3(handle, FFA_SUCCESS_SMC32, 0x0,
+			 spmc_get_hyp_ctx()->ns_ep_id);
+	}
+}
+
+/*
+ * Enable an SP to query the ID assigned to the SPMC.
+ */
+static uint64_t ffa_spm_id_get_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)
+{
+	assert(x1 == 0UL);
+	assert(x2 == 0UL);
+	assert(x3 == 0UL);
+	assert(x4 == 0UL);
+	assert(SMC_GET_GP(handle, CTX_GPREG_X5) == 0UL);
+	assert(SMC_GET_GP(handle, CTX_GPREG_X6) == 0UL);
+	assert(SMC_GET_GP(handle, CTX_GPREG_X7) == 0UL);
+
+	SMC_RET3(handle, FFA_SUCCESS_SMC32, 0x0, FFA_SPMC_ID);
+}
+
+static uint64_t ffa_run_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;
+	uint16_t target_id = FFA_RUN_EP_ID(x1);
+	uint16_t vcpu_id = FFA_RUN_VCPU_ID(x1);
+	unsigned int idx;
+	unsigned int *rt_state;
+	unsigned int *rt_model;
+
+	/* Can only be called from the normal world. */
+	if (secure_origin) {
+		ERROR("FFA_RUN can only be called from NWd.\n");
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Cannot run a Normal world partition. */
+	if (ffa_is_normal_world_id(target_id)) {
+		ERROR("Cannot run a NWd partition (0x%x).\n", target_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Check that the target SP exists. */
+	sp = spmc_get_sp_ctx(target_id);
+		ERROR("Unknown partition ID (0x%x).\n", target_id);
+	if (sp == NULL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	idx = get_ec_index(sp);
+	if (idx != vcpu_id) {
+		ERROR("Cannot run vcpu %d != %d.\n", idx, vcpu_id);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+	rt_state = &((sp->ec[idx]).rt_state);
+	rt_model = &((sp->ec[idx]).rt_model);
+	if (*rt_state == RT_STATE_RUNNING) {
+		ERROR("Partition (0x%x) is already running.\n", target_id);
+		return spmc_ffa_error_return(handle, FFA_ERROR_BUSY);
+	}
+
+	/*
+	 * Sanity check that if the execution context was not waiting then it
+	 * was either in the direct request or the run partition runtime model.
+	 */
+	if (*rt_state == RT_STATE_PREEMPTED || *rt_state == RT_STATE_BLOCKED) {
+		assert(*rt_model == RT_MODEL_RUN ||
+		       *rt_model == RT_MODEL_DIR_REQ);
+	}
+
+	/*
+	 * If the context was waiting then update the partition runtime model.
+	 */
+	if (*rt_state == RT_STATE_WAITING) {
+		*rt_model = RT_MODEL_RUN;
+	}
+
+	/*
+	 * Forward the request to the correct SP vCPU after updating
+	 * its state.
+	 */
+	*rt_state = RT_STATE_RUNNING;
+
+	return spmc_smc_return(smc_fid, secure_origin, x1, 0, 0, 0,
+			       handle, cookie, flags, target_id);
+}
+
+static uint64_t rx_release_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 mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+
+	spin_lock(&mbox->lock);
+
+	if (mbox->state != MAILBOX_STATE_FULL) {
+		spin_unlock(&mbox->lock);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	mbox->state = MAILBOX_STATE_EMPTY;
+	spin_unlock(&mbox->lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+}
+
+/*
+ * Perform initial validation on the provided secondary entry point.
+ * For now ensure it does not lie within the BL31 Image or the SP's
+ * RX/TX buffers as these are mapped within EL3.
+ * TODO: perform validation for additional invalid memory regions.
+ */
+static int validate_secondary_ep(uintptr_t ep, struct secure_partition_desc *sp)
+{
+	struct mailbox *mb;
+	uintptr_t buffer_size;
+	uintptr_t sp_rx_buffer;
+	uintptr_t sp_tx_buffer;
+	uintptr_t sp_rx_buffer_limit;
+	uintptr_t sp_tx_buffer_limit;
+
+	mb = &sp->mailbox;
+	buffer_size = (uintptr_t) (mb->rxtx_page_count * FFA_PAGE_SIZE);
+	sp_rx_buffer = (uintptr_t) mb->rx_buffer;
+	sp_tx_buffer = (uintptr_t) mb->tx_buffer;
+	sp_rx_buffer_limit = sp_rx_buffer + buffer_size;
+	sp_tx_buffer_limit = sp_tx_buffer + buffer_size;
+
+	/*
+	 * Check if the entry point lies within BL31, or the
+	 * SP's RX or TX buffer.
+	 */
+	if ((ep >= BL31_BASE && ep < BL31_LIMIT) ||
+	    (ep >= sp_rx_buffer && ep < sp_rx_buffer_limit) ||
+	    (ep >= sp_tx_buffer && ep < sp_tx_buffer_limit)) {
+		return -EINVAL;
+	}
+	return 0;
+}
+
+/*******************************************************************************
+ * This function handles the FFA_SECONDARY_EP_REGISTER SMC to allow an SP to
+ *  register an entry point for initialization during a secondary cold boot.
+ ******************************************************************************/
+static uint64_t ffa_sec_ep_register_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;
+	struct sp_exec_ctx *sp_ctx;
+
+	/* This request cannot originate from the Normal world. */
+	if (!secure_origin) {
+		WARN("%s: Can only be called from SWd.\n", __func__);
+		return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/* Get the context of the current SP. */
+	sp = spmc_get_current_sp_ctx();
+	if (sp == NULL) {
+		WARN("%s: Cannot find SP context.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Only an S-EL1 SP should be invoking this ABI. */
+	if (sp->runtime_el != S_EL1) {
+		WARN("%s: Can only be called for a S-EL1 SP.\n", __func__);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	/* Ensure the SP is in its initialization state. */
+	sp_ctx = spmc_get_sp_ec(sp);
+	if (sp_ctx->rt_model != RT_MODEL_INIT) {
+		WARN("%s: Can only be called during SP initialization.\n",
+		     __func__);
+		return spmc_ffa_error_return(handle, FFA_ERROR_DENIED);
+	}
+
+	/* Perform initial validation of the secondary entry point. */
+	if (validate_secondary_ep(x1, sp)) {
+		WARN("%s: Invalid entry point provided (0x%lx).\n",
+		     __func__, x1);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Update the secondary entrypoint in SP context.
+	 * We don't need a lock here as during partition initialization there
+	 * will only be a single core online.
+	 */
+	sp->secondary_ep = x1;
+	VERBOSE("%s: 0x%lx\n", __func__, sp->secondary_ep);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
 }
 
 /*******************************************************************************
@@ -574,7 +1394,8 @@
  ******************************************************************************/
 static int sp_manifest_parse(void *sp_manifest, int offset,
 			     struct secure_partition_desc *sp,
-			     entry_point_info_t *ep_info)
+			     entry_point_info_t *ep_info,
+			     int32_t *boot_info_reg)
 {
 	int32_t ret, node;
 	uint32_t config_32;
@@ -672,6 +1493,39 @@
 		sp->sp_id = config_32;
 	}
 
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "power-management-messages", &config_32);
+	if (ret != 0) {
+		WARN("Missing Power Management Messages entry.\n");
+	} else {
+		/*
+		 * Ensure only the currently supported power messages have
+		 * been requested.
+		 */
+		if (config_32 & ~(FFA_PM_MSG_SUB_CPU_OFF |
+				  FFA_PM_MSG_SUB_CPU_SUSPEND |
+				  FFA_PM_MSG_SUB_CPU_SUSPEND_RESUME)) {
+			ERROR("Requested unsupported PM messages (%x)\n",
+			      config_32);
+			return -EINVAL;
+		}
+		sp->pwr_mgmt_msgs = config_32;
+	}
+
+	ret = fdt_read_uint32(sp_manifest, node,
+			      "gp-register-num", &config_32);
+	if (ret != 0) {
+		WARN("Missing boot information register.\n");
+	} else {
+		/* Check if a register number between 0-3 is specified. */
+		if (config_32 < 4) {
+			*boot_info_reg = config_32;
+		} else {
+			WARN("Incorrect boot information register (%u).\n",
+			     config_32);
+		}
+	}
+
 	return 0;
 }
 
@@ -687,7 +1541,7 @@
 	uintptr_t manifest_base;
 	uintptr_t manifest_base_align;
 	entry_point_info_t *next_image_ep_info;
-	int32_t ret;
+	int32_t ret, boot_info_reg = -1;
 	struct secure_partition_desc *sp;
 
 	next_image_ep_info = bl31_plat_get_next_image_ep_info(SECURE);
@@ -746,7 +1600,8 @@
 		       SECURE | EP_ST_ENABLE);
 
 	/* Parse the SP manifest. */
-	ret = sp_manifest_parse(sp_manifest, ret, sp, next_image_ep_info);
+	ret = sp_manifest_parse(sp_manifest, ret, sp, next_image_ep_info,
+				&boot_info_reg);
 	if (ret != 0) {
 		ERROR("Error in Secure Partition manifest parsing.\n");
 		return ret;
@@ -759,7 +1614,7 @@
 	}
 
 	/* Perform any common initialisation. */
-	spmc_sp_common_setup(sp, next_image_ep_info);
+	spmc_sp_common_setup(sp, next_image_ep_info, boot_info_reg);
 
 	/* Perform any initialisation specific to S-EL1 SPs. */
 	spmc_el1_sp_setup(sp, next_image_ep_info);
@@ -910,11 +1765,24 @@
 int32_t spmc_setup(void)
 {
 	int32_t ret;
+	uint32_t flags;
 
 	/* Initialize endpoint descriptors */
 	initalize_sp_descs();
 	initalize_ns_ep_descs();
 
+	/*
+	 * Retrieve the information of the datastore for tracking shared memory
+	 * requests allocated by platform code and zero the region if available.
+	 */
+	ret = plat_spmc_shmem_datastore_get(&spmc_shmem_obj_state.data,
+					    &spmc_shmem_obj_state.data_size);
+	if (ret != 0) {
+		ERROR("Failed to obtain memory descriptor backing store!\n");
+		return ret;
+	}
+	memset(spmc_shmem_obj_state.data, 0, spmc_shmem_obj_state.data_size);
+
 	/* Setup logical SPs. */
 	ret = logical_sp_init();
 	if (ret != 0) {
@@ -936,6 +1804,24 @@
 		return ret;
 	}
 
+	/* Register power management hooks with PSCI */
+	psci_register_spd_pm_hook(&spmc_pm);
+
+	/*
+	 * Register an interrupt handler for S-EL1 interrupts
+	 * when generated during code executing in the
+	 * non-secure state.
+	 */
+	flags = 0;
+	set_interrupt_rm_flag(flags, NON_SECURE);
+	ret = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+					      spmc_sp_interrupt_handler,
+					      flags);
+	if (ret != 0) {
+		ERROR("Failed to register interrupt handler! (%d)\n", ret);
+		panic();
+	}
+
 	/* Register init function for deferred init.  */
 	bl31_register_bl32_init(&sp_init);
 
@@ -963,6 +1849,23 @@
 		return ffa_version_handler(smc_fid, secure_origin, x1, x2, x3,
 					   x4, cookie, handle, flags);
 
+	case FFA_SPM_ID_GET:
+		return ffa_spm_id_get_handler(smc_fid, secure_origin, x1, x2,
+					     x3, x4, cookie, handle, flags);
+
+	case FFA_ID_GET:
+		return ffa_id_get_handler(smc_fid, secure_origin, x1, x2, x3,
+					  x4, cookie, handle, flags);
+
+	case FFA_FEATURES:
+		return ffa_features_handler(smc_fid, secure_origin, x1, x2, x3,
+					    x4, cookie, handle, flags);
+
+	case FFA_SECONDARY_EP_REGISTER_SMC64:
+		return ffa_sec_ep_register_handler(smc_fid, secure_origin, x1,
+						   x2, x3, x4, cookie, handle,
+						   flags);
+
 	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,
@@ -973,6 +1876,24 @@
 		return direct_resp_smc_handler(smc_fid, secure_origin, x1, x2,
 					       x3, x4, cookie, handle, flags);
 
+	case FFA_RXTX_MAP_SMC32:
+	case FFA_RXTX_MAP_SMC64:
+		return rxtx_map_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+
+	case FFA_RXTX_UNMAP:
+		return rxtx_unmap_handler(smc_fid, secure_origin, x1, x2, x3,
+					  x4, cookie, handle, flags);
+
+	case FFA_PARTITION_INFO_GET:
+		return partition_info_get_handler(smc_fid, secure_origin, x1,
+						  x2, x3, x4, cookie, handle,
+						  flags);
+
+	case FFA_RX_RELEASE:
+		return rx_release_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);
@@ -981,9 +1902,94 @@
 		return ffa_error_handler(smc_fid, secure_origin, x1, x2, x3, x4,
 					cookie, handle, flags);
 
+	case FFA_MSG_RUN:
+		return ffa_run_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+				       cookie, handle, flags);
+
+	case FFA_MEM_SHARE_SMC32:
+	case FFA_MEM_SHARE_SMC64:
+	case FFA_MEM_LEND_SMC32:
+	case FFA_MEM_LEND_SMC64:
+		return spmc_ffa_mem_send(smc_fid, secure_origin, x1, x2, x3, x4,
+					 cookie, handle, flags);
+
+	case FFA_MEM_FRAG_TX:
+		return spmc_ffa_mem_frag_tx(smc_fid, secure_origin, x1, x2, x3,
+					    x4, cookie, handle, flags);
+
+	case FFA_MEM_FRAG_RX:
+		return spmc_ffa_mem_frag_rx(smc_fid, secure_origin, x1, x2, x3,
+					    x4, cookie, handle, flags);
+
+	case FFA_MEM_RETRIEVE_REQ_SMC32:
+	case FFA_MEM_RETRIEVE_REQ_SMC64:
+		return spmc_ffa_mem_retrieve_req(smc_fid, secure_origin, x1, x2,
+						 x3, x4, cookie, handle, flags);
+
+	case FFA_MEM_RELINQUISH:
+		return spmc_ffa_mem_relinquish(smc_fid, secure_origin, x1, x2,
+					       x3, x4, cookie, handle, flags);
+
+	case FFA_MEM_RECLAIM:
+		return spmc_ffa_mem_reclaim(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);
 }
+
+/*******************************************************************************
+ * This function is the handler registered for S-EL1 interrupts by the SPMC. It
+ * validates the interrupt and upon success arranges entry into the SP for
+ * handling the interrupt.
+ ******************************************************************************/
+static uint64_t spmc_sp_interrupt_handler(uint32_t id,
+					  uint32_t flags,
+					  void *handle,
+					  void *cookie)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	struct sp_exec_ctx *ec;
+	uint32_t linear_id = plat_my_core_pos();
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/* Check the security state when the exception was generated. */
+	assert(get_interrupt_src_ss(flags) == NON_SECURE);
+
+	/* Panic if not an S-EL1 Partition. */
+	if (sp->runtime_el != S_EL1) {
+		ERROR("Interrupt received for a non S-EL1 SP on core%u.\n",
+		      linear_id);
+		panic();
+	}
+
+	/* Obtain a reference to the SP execution context. */
+	ec = spmc_get_sp_ec(sp);
+
+	/* Ensure that the execution context is in waiting state else panic. */
+	if (ec->rt_state != RT_STATE_WAITING) {
+		ERROR("SP EC on core%u is not waiting (%u), it is (%u).\n",
+		      linear_id, RT_STATE_WAITING, ec->rt_state);
+		panic();
+	}
+
+	/* Update the runtime model and state of the partition. */
+	ec->rt_model = RT_MODEL_INTR;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	VERBOSE("SP (0x%x) interrupt start on core%u.\n", sp->sp_id, linear_id);
+
+	/*
+	 * Forward the interrupt to the S-EL1 SP. The interrupt ID is not
+	 * populated as the SP can determine this by itself.
+	 */
+	return spmd_smc_switch_state(FFA_INTERRUPT, false,
+				     FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+				     FFA_PARAM_MBZ, FFA_PARAM_MBZ,
+				     handle);
+}
diff --git a/services/std_svc/spm/el3_spmc/spmc_pm.c b/services/std_svc/spm/el3_spmc/spmc_pm.c
new file mode 100644
index 0000000..d25344c
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_pm.c
@@ -0,0 +1,283 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <lib/el3_runtime/context_mgmt.h>
+#include <lib/spinlock.h>
+#include <plat/common/common_def.h>
+#include <plat/common/platform.h>
+#include <services/ffa_svc.h>
+#include "spmc.h"
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * spmc_build_pm_message
+ *
+ * Builds an SPMC to SP direct message request.
+ ******************************************************************************/
+static void spmc_build_pm_message(gp_regs_t *gpregs,
+				  unsigned long long message,
+				  uint8_t pm_msg_type,
+				  uint16_t sp_id)
+{
+	write_ctx_reg(gpregs, CTX_GPREG_X0, FFA_MSG_SEND_DIRECT_REQ_SMC32);
+	write_ctx_reg(gpregs, CTX_GPREG_X1,
+		      (FFA_SPMC_ID << FFA_DIRECT_MSG_SOURCE_SHIFT) |
+		      sp_id);
+	write_ctx_reg(gpregs, CTX_GPREG_X2, FFA_FWK_MSG_BIT |
+		      (pm_msg_type & FFA_FWK_MSG_MASK));
+	write_ctx_reg(gpregs, CTX_GPREG_X3, message);
+}
+
+/*******************************************************************************
+ * This CPU has been turned on. Enter the SP to initialise S-EL1.
+ ******************************************************************************/
+static void spmc_cpu_on_finish_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	struct sp_exec_ctx *ec;
+	unsigned int linear_id = plat_my_core_pos();
+	entry_point_info_t sec_ec_ep_info = {0};
+	uint64_t rc;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/* Initialize entry point information for the SP. */
+	SET_PARAM_HEAD(&sec_ec_ep_info, PARAM_EP, VERSION_1,
+		       SECURE | EP_ST_ENABLE);
+
+	/*
+	 * Check if the primary execution context registered an entry point else
+	 * bail out early.
+	 * TODO: Add support for boot reason in manifest to allow jumping to
+	 * entrypoint into the primary execution context.
+	 */
+	if (sp->secondary_ep == 0) {
+		WARN("%s: No secondary ep on core%u\n", __func__, linear_id);
+		return;
+	}
+
+	sec_ec_ep_info.pc = sp->secondary_ep;
+
+	/*
+	 * Setup and initialise the SP execution context on this physical cpu.
+	 */
+	spmc_el1_sp_setup(sp, &sec_ec_ep_info);
+	spmc_sp_common_ep_commit(sp, &sec_ec_ep_info);
+
+	/* Obtain a reference to the SP execution context. */
+	ec = spmc_get_sp_ec(sp);
+
+	/*
+	 * TODO: Should we do some PM related state tracking of the SP execution
+	 * context here?
+	 */
+
+	/* Update the runtime model and state of the partition. */
+	ec->rt_model = RT_MODEL_INIT;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	INFO("SP (0x%x) init start on core%u.\n", sp->sp_id, linear_id);
+
+	rc = spmc_sp_synchronous_entry(ec);
+	if (rc != 0ULL) {
+		ERROR("%s failed (%lu) on CPU%u\n", __func__, rc, linear_id);
+	}
+
+	/* Update the runtime state of the partition. */
+	ec->rt_state = RT_STATE_WAITING;
+
+	VERBOSE("CPU %u on!\n", linear_id);
+}
+/*******************************************************************************
+ * Helper function to send a FF-A power management message to an SP.
+ ******************************************************************************/
+static int32_t spmc_send_pm_msg(uint8_t pm_msg_type,
+				unsigned long long psci_event)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	struct sp_exec_ctx *ec;
+	gp_regs_t *gpregs_ctx;
+	unsigned int linear_id = plat_my_core_pos();
+	u_register_t resp;
+	uint64_t rc;
+
+	/* Obtain a reference to the SP execution context. */
+	ec = spmc_get_sp_ec(sp);
+
+	/*
+	 * TODO: Should we do some PM related state tracking of the SP execution
+	 * context here?
+	 */
+
+	/*
+	 * Build an SPMC to SP direct message request.
+	 * Note that x4-x6 should be populated with the original PSCI arguments.
+	 */
+	spmc_build_pm_message(get_gpregs_ctx(&ec->cpu_ctx),
+			      psci_event,
+			      pm_msg_type,
+			      sp->sp_id);
+
+	/* Sanity check partition state. */
+	assert(ec->rt_state == RT_STATE_WAITING);
+
+	/* Update the runtime model and state of the partition. */
+	ec->rt_model = RT_MODEL_DIR_REQ;
+	ec->rt_state = RT_STATE_RUNNING;
+
+	rc = spmc_sp_synchronous_entry(ec);
+	if (rc != 0ULL) {
+		ERROR("%s failed (%lu) on CPU%u.\n", __func__, rc, linear_id);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/*
+	 * Validate we receive an expected response from the SP.
+	 * TODO: We don't currently support aborting an SP in the scenario
+	 * where it is misbehaving so assert these conditions are not
+	 * met for now.
+	 */
+	gpregs_ctx = get_gpregs_ctx(&ec->cpu_ctx);
+
+	/* Expect a direct message response from the SP. */
+	resp = read_ctx_reg(gpregs_ctx, CTX_GPREG_X0);
+	if (resp != FFA_MSG_SEND_DIRECT_RESP_SMC32) {
+		ERROR("%s invalid SP response (%lx).\n", __func__, resp);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/* Ensure the sender and receiver are populated correctly. */
+	resp = read_ctx_reg(gpregs_ctx, CTX_GPREG_X1);
+	if (!(ffa_endpoint_source(resp) == sp->sp_id &&
+	      ffa_endpoint_destination(resp) == FFA_SPMC_ID)) {
+		ERROR("%s invalid src/dst response (%lx).\n", __func__, resp);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/* Expect a PM message response from the SP. */
+	resp = read_ctx_reg(gpregs_ctx, CTX_GPREG_X2);
+	if ((resp & FFA_FWK_MSG_BIT) == 0U ||
+	    ((resp & FFA_FWK_MSG_MASK) != FFA_PM_MSG_PM_RESP)) {
+		ERROR("%s invalid PM response (%lx).\n", __func__, resp);
+		assert(false);
+		return -EINVAL;
+	}
+
+	/* Update the runtime state of the partition. */
+	ec->rt_state = RT_STATE_WAITING;
+
+	/* Return the status code returned by the SP */
+	return read_ctx_reg(gpregs_ctx, CTX_GPREG_X3);
+}
+
+/*******************************************************************************
+ * spmc_cpu_suspend_finish_handler
+ ******************************************************************************/
+static void spmc_cpu_suspend_finish_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	unsigned int linear_id = plat_my_core_pos();
+	int32_t rc;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/*
+	 * Check if the SP has subscribed for this power management message.
+	 * If not then we don't have anything else to do here.
+	 */
+	if ((sp->pwr_mgmt_msgs & FFA_PM_MSG_SUB_CPU_SUSPEND_RESUME) == 0U) {
+		goto exit;
+	}
+
+	rc = spmc_send_pm_msg(FFA_PM_MSG_WB_REQ, FFA_WB_TYPE_NOTS2RAM);
+	if (rc < 0) {
+		ERROR("%s failed (%d) on CPU%u\n", __func__, rc, linear_id);
+		return;
+	}
+
+exit:
+	VERBOSE("CPU %u resumed!\n", linear_id);
+}
+
+/*******************************************************************************
+ * spmc_cpu_suspend_handler
+ ******************************************************************************/
+static void spmc_cpu_suspend_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	unsigned int linear_id = plat_my_core_pos();
+	int32_t rc;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/*
+	 * Check if the SP has subscribed for this power management message.
+	 * If not then we don't have anything else to do here.
+	 */
+	if ((sp->pwr_mgmt_msgs & FFA_PM_MSG_SUB_CPU_SUSPEND) == 0U) {
+		goto exit;
+	}
+
+	rc = spmc_send_pm_msg(FFA_FWK_MSG_PSCI, PSCI_CPU_SUSPEND_AARCH64);
+	if (rc < 0) {
+		ERROR("%s failed (%d) on CPU%u\n", __func__, rc, linear_id);
+		return;
+	}
+exit:
+	VERBOSE("CPU %u suspend!\n", linear_id);
+}
+
+/*******************************************************************************
+ * spmc_cpu_off_handler
+ ******************************************************************************/
+static int32_t spmc_cpu_off_handler(u_register_t unused)
+{
+	struct secure_partition_desc *sp = spmc_get_current_sp_ctx();
+	unsigned int linear_id = plat_my_core_pos();
+	int32_t ret = 0;
+
+	/* Sanity check for a NULL pointer dereference. */
+	assert(sp != NULL);
+
+	/*
+	 * Check if the SP has subscribed for this power management message.
+	 * If not then we don't have anything else to do here.
+	 */
+	if ((sp->pwr_mgmt_msgs & FFA_PM_MSG_SUB_CPU_OFF) == 0U) {
+		goto exit;
+	}
+
+	ret = spmc_send_pm_msg(FFA_FWK_MSG_PSCI, PSCI_CPU_OFF);
+	if (ret < 0) {
+		ERROR("%s failed (%d) on CPU%u\n", __func__, ret, linear_id);
+		return ret;
+	}
+
+exit:
+	VERBOSE("CPU %u off!\n", linear_id);
+	return ret;
+}
+
+/*******************************************************************************
+ * Structure populated by the SPM Core to perform any bookkeeping before
+ * PSCI executes a power mgmt. operation.
+ ******************************************************************************/
+const spd_pm_ops_t spmc_pm = {
+	.svc_on_finish = spmc_cpu_on_finish_handler,
+	.svc_off = spmc_cpu_off_handler,
+	.svc_suspend = spmc_cpu_suspend_handler,
+	.svc_suspend_finish = spmc_cpu_suspend_finish_handler
+};
diff --git a/services/std_svc/spm/el3_spmc/spmc_setup.c b/services/std_svc/spm/el3_spmc/spmc_setup.c
index 7b23c9e..8ebae28 100644
--- a/services/std_svc/spm/el3_spmc/spmc_setup.c
+++ b/services/std_svc/spm/el3_spmc/spmc_setup.c
@@ -10,19 +10,139 @@
 #include <arch.h>
 #include <arch_helpers.h>
 #include <common/debug.h>
+#include <common/fdt_wrappers.h>
 #include <context.h>
 #include <lib/el3_runtime/context_mgmt.h>
 #include <lib/utils.h>
 #include <lib/xlat_tables/xlat_tables_v2.h>
+#include <libfdt.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 <tools_share/firmware_image_package.h>
 
 #include <platform_def.h>
 
 /*
+ * Statically allocate a page of memory for passing boot information to an SP.
+ */
+static uint8_t ffa_boot_info_mem[PAGE_SIZE] __aligned(PAGE_SIZE);
+
+/*
+ * This function creates a initialization descriptor in the memory reserved
+ * for passing boot information to an SP. It then copies the partition manifest
+ * into this region and ensures that its reference in the initialization
+ * descriptor is updated.
+ */
+static void spmc_create_boot_info(entry_point_info_t *ep_info,
+				  struct secure_partition_desc *sp)
+{
+	struct ffa_boot_info_header *boot_header;
+	struct ffa_boot_info_desc *boot_descriptor;
+	uintptr_t manifest_addr;
+
+	/*
+	 * Calculate the maximum size of the manifest that can be accommodated
+	 * in the boot information memory region.
+	 */
+	const unsigned int
+	max_manifest_sz = sizeof(ffa_boot_info_mem) -
+			  (sizeof(struct ffa_boot_info_header) +
+			   sizeof(struct ffa_boot_info_desc));
+
+	/*
+	 * The current implementation only supports the FF-A v1.1
+	 * implementation of the boot protocol, therefore check
+	 * that a v1.0 SP has not requested use of the protocol.
+	 */
+	if (sp->ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		ERROR("FF-A boot protocol not supported for v1.0 clients\n");
+		return;
+	}
+
+	/*
+	 * Check if the manifest will fit into the boot info memory region else
+	 * bail.
+	 */
+	if (ep_info->args.arg1 > max_manifest_sz) {
+		WARN("Unable to copy manifest into boot information. ");
+		WARN("Max sz = %u bytes. Manifest sz = %lu bytes\n",
+		     max_manifest_sz, ep_info->args.arg1);
+		return;
+	}
+
+	/* Zero the memory region before populating. */
+	memset(ffa_boot_info_mem, 0, PAGE_SIZE);
+
+	/*
+	 * Populate the ffa_boot_info_header at the start of the boot info
+	 * region.
+	 */
+	boot_header = (struct ffa_boot_info_header *) ffa_boot_info_mem;
+
+	/* Position the ffa_boot_info_desc after the ffa_boot_info_header. */
+	boot_header->offset_boot_info_desc =
+					sizeof(struct ffa_boot_info_header);
+	boot_descriptor = (struct ffa_boot_info_desc *)
+			  (ffa_boot_info_mem +
+			   boot_header->offset_boot_info_desc);
+
+	/*
+	 * We must use the FF-A version coresponding to the version implemented
+	 * by the SP. Currently this can only be v1.1.
+	 */
+	boot_header->version = sp->ffa_version;
+
+	/* Populate the boot information header. */
+	boot_header->size_boot_info_desc = sizeof(struct ffa_boot_info_desc);
+
+	/* Set the signature "0xFFA". */
+	boot_header->signature = FFA_INIT_DESC_SIGNATURE;
+
+	/* Set the count. Currently 1 since only the manifest is specified. */
+	boot_header->count_boot_info_desc = 1;
+
+	/* Populate the boot information descriptor for the manifest. */
+	boot_descriptor->type =
+		FFA_BOOT_INFO_TYPE(FFA_BOOT_INFO_TYPE_STD) |
+		FFA_BOOT_INFO_TYPE_ID(FFA_BOOT_INFO_TYPE_ID_FDT);
+
+	boot_descriptor->flags =
+		FFA_BOOT_INFO_FLAG_NAME(FFA_BOOT_INFO_FLAG_NAME_UUID) |
+		FFA_BOOT_INFO_FLAG_CONTENT(FFA_BOOT_INFO_FLAG_CONTENT_ADR);
+
+	/*
+	 * Copy the manifest into boot info region after the boot information
+	 * descriptor.
+	 */
+	boot_descriptor->size_boot_info = (uint32_t) ep_info->args.arg1;
+
+	manifest_addr = (uintptr_t) (ffa_boot_info_mem +
+				     boot_header->offset_boot_info_desc +
+				     boot_header->size_boot_info_desc);
+
+	memcpy((void *) manifest_addr, (void *) ep_info->args.arg0,
+	       boot_descriptor->size_boot_info);
+
+	boot_descriptor->content = manifest_addr;
+
+	/* Calculate the size of the total boot info blob. */
+	boot_header->size_boot_info_blob = boot_header->offset_boot_info_desc +
+					   boot_descriptor->size_boot_info +
+					   (boot_header->count_boot_info_desc *
+					    boot_header->size_boot_info_desc);
+
+	INFO("SP boot info @ 0x%lx, size: %u bytes.\n",
+	     (uintptr_t) ffa_boot_info_mem,
+	     boot_header->size_boot_info_blob);
+	INFO("SP manifest @ 0x%lx, size: %u bytes.\n",
+	     boot_descriptor->content,
+	     boot_descriptor->size_boot_info);
+}
+
+/*
  * We are assuming that the index of the execution
  * context used is the linear index of the current physical cpu.
  */
@@ -44,6 +164,12 @@
 				DISABLE_ALL_EXCEPTIONS);
 
 	/*
+	 * TF-A Implementation defined behaviour to provide the linear
+	 * core ID in the x4 register.
+	 */
+	ep_info->args.arg4 = (uintptr_t) plat_my_core_pos();
+
+	/*
 	 * 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.
@@ -62,7 +188,8 @@
 
 /* Common initialisation for all SPs. */
 void spmc_sp_common_setup(struct secure_partition_desc *sp,
-			  entry_point_info_t *ep_info)
+			  entry_point_info_t *ep_info,
+			  int32_t boot_info_reg)
 {
 	uint16_t sp_id;
 
@@ -90,11 +217,50 @@
 	 */
 	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));
+	/* Check if the SP wants to use the FF-A boot protocol. */
+	if (boot_info_reg >= 0) {
+		/*
+		 * Create a boot information descriptor and copy the partition
+		 * manifest into the reserved memory region for consumption by
+		 * the SP.
+		 */
+		spmc_create_boot_info(ep_info, sp);
+
+		/*
+		 * We have consumed what we need from ep args so we can now
+		 * zero them before we start populating with new information
+		 * specifically for the SP.
+		 */
+		zeromem(&ep_info->args, sizeof(ep_info->args));
+
+		/*
+		 * Pass the address of the boot information in the
+		 * boot_info_reg.
+		 */
+		switch (boot_info_reg) {
+		case 0:
+			ep_info->args.arg0 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		case 1:
+			ep_info->args.arg1 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		case 2:
+			ep_info->args.arg2 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		case 3:
+			ep_info->args.arg3 = (uintptr_t) ffa_boot_info_mem;
+			break;
+		default:
+			ERROR("Invalid value for \"gp-register-num\" %d.\n",
+			      boot_info_reg);
+		}
+	} else {
+		/*
+		 * We don't need any of the information that was populated
+		 * in ep_args so we can clear them.
+		 */
+		zeromem(&ep_info->args, sizeof(ep_info->args));
+	}
 }
 
 /*
diff --git a/services/std_svc/spm/el3_spmc/spmc_shared_mem.c b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
new file mode 100644
index 0000000..1602981
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_shared_mem.c
@@ -0,0 +1,1812 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/object_pool.h>
+#include <lib/spinlock.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <services/ffa_svc.h>
+#include "spmc.h"
+#include "spmc_shared_mem.h"
+
+#include <platform_def.h>
+
+/**
+ * struct spmc_shmem_obj - Shared memory object.
+ * @desc_size:      Size of @desc.
+ * @desc_filled:    Size of @desc already received.
+ * @in_use:         Number of clients that have called ffa_mem_retrieve_req
+ *                  without a matching ffa_mem_relinquish call.
+ * @desc:           FF-A memory region descriptor passed in ffa_mem_share.
+ */
+struct spmc_shmem_obj {
+	size_t desc_size;
+	size_t desc_filled;
+	size_t in_use;
+	struct ffa_mtd desc;
+};
+
+/*
+ * Declare our data structure to store the metadata of memory share requests.
+ * The main datastore is allocated on a per platform basis to ensure enough
+ * storage can be made available.
+ * The address of the data store will be populated by the SPMC during its
+ * initialization.
+ */
+
+struct spmc_shmem_obj_state spmc_shmem_obj_state = {
+	/* Set start value for handle so top 32 bits are needed quickly. */
+	.next_handle = 0xffffffc0U,
+};
+
+/**
+ * spmc_shmem_obj_size - Convert from descriptor size to object size.
+ * @desc_size:  Size of struct ffa_memory_region_descriptor object.
+ *
+ * Return: Size of struct spmc_shmem_obj object.
+ */
+static size_t spmc_shmem_obj_size(size_t desc_size)
+{
+	return desc_size + offsetof(struct spmc_shmem_obj, desc);
+}
+
+/**
+ * spmc_shmem_obj_alloc - Allocate struct spmc_shmem_obj.
+ * @state:      Global state.
+ * @desc_size:  Size of struct ffa_memory_region_descriptor object that
+ *              allocated object will hold.
+ *
+ * Return: Pointer to newly allocated object, or %NULL if there not enough space
+ *         left. The returned pointer is only valid while @state is locked, to
+ *         used it again after unlocking @state, spmc_shmem_obj_lookup must be
+ *         called.
+ */
+static struct spmc_shmem_obj *
+spmc_shmem_obj_alloc(struct spmc_shmem_obj_state *state, size_t desc_size)
+{
+	struct spmc_shmem_obj *obj;
+	size_t free = state->data_size - state->allocated;
+
+	if (state->data == NULL) {
+		ERROR("Missing shmem datastore!\n");
+		return NULL;
+	}
+
+	if (spmc_shmem_obj_size(desc_size) > free) {
+		WARN("%s(0x%zx) failed, free 0x%zx\n",
+		     __func__, desc_size, free);
+		return NULL;
+	}
+	obj = (struct spmc_shmem_obj *)(state->data + state->allocated);
+	obj->desc = (struct ffa_mtd) {0};
+	obj->desc_size = desc_size;
+	obj->desc_filled = 0;
+	obj->in_use = 0;
+	state->allocated += spmc_shmem_obj_size(desc_size);
+	return obj;
+}
+
+/**
+ * spmc_shmem_obj_free - Free struct spmc_shmem_obj.
+ * @state:      Global state.
+ * @obj:        Object to free.
+ *
+ * Release memory used by @obj. Other objects may move, so on return all
+ * pointers to struct spmc_shmem_obj object should be considered invalid, not
+ * just @obj.
+ *
+ * The current implementation always compacts the remaining objects to simplify
+ * the allocator and to avoid fragmentation.
+ */
+
+static void spmc_shmem_obj_free(struct spmc_shmem_obj_state *state,
+				  struct spmc_shmem_obj *obj)
+{
+	size_t free_size = spmc_shmem_obj_size(obj->desc_size);
+	uint8_t *shift_dest = (uint8_t *)obj;
+	uint8_t *shift_src = shift_dest + free_size;
+	size_t shift_size = state->allocated - (shift_src - state->data);
+
+	if (shift_size != 0U) {
+		memmove(shift_dest, shift_src, shift_size);
+	}
+	state->allocated -= free_size;
+}
+
+/**
+ * spmc_shmem_obj_lookup - Lookup struct spmc_shmem_obj by handle.
+ * @state:      Global state.
+ * @handle:     Unique handle of object to return.
+ *
+ * Return: struct spmc_shmem_obj_state object with handle matching @handle.
+ *         %NULL, if not object in @state->data has a matching handle.
+ */
+static struct spmc_shmem_obj *
+spmc_shmem_obj_lookup(struct spmc_shmem_obj_state *state, uint64_t handle)
+{
+	uint8_t *curr = state->data;
+
+	while (curr - state->data < state->allocated) {
+		struct spmc_shmem_obj *obj = (struct spmc_shmem_obj *)curr;
+
+		if (obj->desc.handle == handle) {
+			return obj;
+		}
+		curr += spmc_shmem_obj_size(obj->desc_size);
+	}
+	return NULL;
+}
+
+/**
+ * spmc_shmem_obj_get_next - Get the next memory object from an offset.
+ * @offset:     Offset used to track which objects have previously been
+ *              returned.
+ *
+ * Return: the next struct spmc_shmem_obj_state object from the provided
+ *	   offset.
+ *	   %NULL, if there are no more objects.
+ */
+static struct spmc_shmem_obj *
+spmc_shmem_obj_get_next(struct spmc_shmem_obj_state *state, size_t *offset)
+{
+	uint8_t *curr = state->data + *offset;
+
+	if (curr - state->data < state->allocated) {
+		struct spmc_shmem_obj *obj = (struct spmc_shmem_obj *)curr;
+
+		*offset += spmc_shmem_obj_size(obj->desc_size);
+
+		return obj;
+	}
+	return NULL;
+}
+
+/*******************************************************************************
+ * FF-A memory descriptor helper functions.
+ ******************************************************************************/
+/**
+ * spmc_shmem_obj_get_emad - Get the emad from a given index depending on the
+ *                           clients FF-A version.
+ * @desc:         The memory transaction descriptor.
+ * @index:        The index of the emad element to be accessed.
+ * @ffa_version:  FF-A version of the provided structure.
+ * @emad_size:    Will be populated with the size of the returned emad
+ *                descriptor.
+ * Return: A pointer to the requested emad structure.
+ */
+static void *
+spmc_shmem_obj_get_emad(const struct ffa_mtd *desc, uint32_t index,
+			uint32_t ffa_version, size_t *emad_size)
+{
+	uint8_t *emad;
+	/*
+	 * If the caller is using FF-A v1.0 interpret the descriptor as a v1.0
+	 * format, otherwise assume it is a v1.1 format.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		/* Cast our descriptor to the v1.0 format. */
+		struct ffa_mtd_v1_0 *mtd_v1_0 =
+					(struct ffa_mtd_v1_0 *) desc;
+		emad = (uint8_t *)  &(mtd_v1_0->emad);
+		*emad_size = sizeof(struct ffa_emad_v1_0);
+	} else {
+		if (!is_aligned(desc->emad_offset, 16)) {
+			WARN("Emad offset is not aligned.\n");
+			return NULL;
+		}
+		emad = ((uint8_t *) desc + desc->emad_offset);
+		*emad_size = desc->emad_size;
+	}
+	return (emad + (*emad_size * index));
+}
+
+/**
+ * spmc_shmem_obj_get_comp_mrd - Get comp_mrd from a mtd struct based on the
+ *				 FF-A version of the descriptor.
+ * @obj:    Object containing ffa_memory_region_descriptor.
+ *
+ * Return: struct ffa_comp_mrd object corresponding to the composite memory
+ *	   region descriptor.
+ */
+static struct ffa_comp_mrd *
+spmc_shmem_obj_get_comp_mrd(struct spmc_shmem_obj *obj, uint32_t ffa_version)
+{
+	size_t emad_size;
+	/*
+	 * The comp_mrd_offset field of the emad descriptor remains consistent
+	 * between FF-A versions therefore we can use the v1.0 descriptor here
+	 * in all cases.
+	 */
+	struct ffa_emad_v1_0 *emad = spmc_shmem_obj_get_emad(&obj->desc, 0,
+							     ffa_version,
+							     &emad_size);
+	/* Ensure the emad array was found. */
+	if (emad == NULL) {
+		return NULL;
+	}
+
+	/* Ensure the composite descriptor offset is aligned. */
+	if (!is_aligned(emad->comp_mrd_offset, 8)) {
+		WARN("Unaligned composite memory region descriptor offset.\n");
+		return NULL;
+	}
+
+	return (struct ffa_comp_mrd *)
+	       ((uint8_t *)(&obj->desc) + emad->comp_mrd_offset);
+}
+
+/**
+ * spmc_shmem_obj_ffa_constituent_size - Calculate variable size part of obj.
+ * @obj:    Object containing ffa_memory_region_descriptor.
+ *
+ * Return: Size of ffa_constituent_memory_region_descriptors in @obj.
+ */
+static size_t
+spmc_shmem_obj_ffa_constituent_size(struct spmc_shmem_obj *obj,
+				    uint32_t ffa_version)
+{
+	struct ffa_comp_mrd *comp_mrd;
+
+	comp_mrd = spmc_shmem_obj_get_comp_mrd(obj, ffa_version);
+	if (comp_mrd == NULL) {
+		return 0;
+	}
+	return comp_mrd->address_range_count * sizeof(struct ffa_cons_mrd);
+}
+
+/*
+ * Compare two memory regions to determine if any range overlaps with another
+ * ongoing memory transaction.
+ */
+static bool
+overlapping_memory_regions(struct ffa_comp_mrd *region1,
+			   struct ffa_comp_mrd *region2)
+{
+	uint64_t region1_start;
+	uint64_t region1_size;
+	uint64_t region1_end;
+	uint64_t region2_start;
+	uint64_t region2_size;
+	uint64_t region2_end;
+
+	assert(region1 != NULL);
+	assert(region2 != NULL);
+
+	if (region1 == region2) {
+		return true;
+	}
+
+	/*
+	 * Check each memory region in the request against existing
+	 * transactions.
+	 */
+	for (size_t i = 0; i < region1->address_range_count; i++) {
+
+		region1_start = region1->address_range_array[i].address;
+		region1_size =
+			region1->address_range_array[i].page_count *
+			PAGE_SIZE_4KB;
+		region1_end = region1_start + region1_size;
+
+		for (size_t j = 0; j < region2->address_range_count; j++) {
+
+			region2_start = region2->address_range_array[j].address;
+			region2_size =
+				region2->address_range_array[j].page_count *
+				PAGE_SIZE_4KB;
+			region2_end = region2_start + region2_size;
+
+			if ((region1_start >= region2_start &&
+			     region1_start < region2_end) ||
+			    (region1_end >= region2_start
+			     && region1_end < region2_end)) {
+				WARN("Overlapping mem regions 0x%lx-0x%lx & 0x%lx-0x%lx\n",
+				     region1_start, region1_end,
+				     region2_start, region2_end);
+				return true;
+			}
+		}
+	}
+	return false;
+}
+
+/*******************************************************************************
+ * FF-A v1.0 Memory Descriptor Conversion Helpers.
+ ******************************************************************************/
+/**
+ * spmc_shm_get_v1_1_descriptor_size - Calculate the required size for a v1.1
+ *                                     converted descriptor.
+ * @orig:       The original v1.0 memory transaction descriptor.
+ * @desc_size:  The size of the original v1.0 memory transaction descriptor.
+ *
+ * Return: the size required to store the descriptor store in the v1.1 format.
+ */
+static size_t
+spmc_shm_get_v1_1_descriptor_size(struct ffa_mtd_v1_0 *orig, size_t desc_size)
+{
+	size_t size = 0;
+	struct ffa_comp_mrd *mrd;
+	struct ffa_emad_v1_0 *emad_array = orig->emad;
+
+	/* Get the size of the v1.1 descriptor. */
+	size += sizeof(struct ffa_mtd);
+
+	/* Add the size of the emad descriptors. */
+	size += orig->emad_count * sizeof(struct ffa_emad_v1_0);
+
+	/* Add the size of the composite mrds. */
+	size += sizeof(struct ffa_comp_mrd);
+
+	/* Add the size of the constituent mrds. */
+	mrd = (struct ffa_comp_mrd *) ((uint8_t *) orig +
+	      emad_array[0].comp_mrd_offset);
+
+	/* Check the calculated address is within the memory descriptor. */
+	if ((uintptr_t) mrd >= (uintptr_t)((uint8_t *) orig + desc_size)) {
+		return 0;
+	}
+	size += mrd->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	return size;
+}
+
+/**
+ * spmc_shm_get_v1_0_descriptor_size - Calculate the required size for a v1.0
+ *                                     converted descriptor.
+ * @orig:       The original v1.1 memory transaction descriptor.
+ * @desc_size:  The size of the original v1.1 memory transaction descriptor.
+ *
+ * Return: the size required to store the descriptor store in the v1.0 format.
+ */
+static size_t
+spmc_shm_get_v1_0_descriptor_size(struct ffa_mtd *orig, size_t desc_size)
+{
+	size_t size = 0;
+	struct ffa_comp_mrd *mrd;
+	struct ffa_emad_v1_0 *emad_array = (struct ffa_emad_v1_0 *)
+					   ((uint8_t *) orig +
+					    orig->emad_offset);
+
+	/* Get the size of the v1.0 descriptor. */
+	size += sizeof(struct ffa_mtd_v1_0);
+
+	/* Add the size of the v1.0 emad descriptors. */
+	size += orig->emad_count * sizeof(struct ffa_emad_v1_0);
+
+	/* Add the size of the composite mrds. */
+	size += sizeof(struct ffa_comp_mrd);
+
+	/* Add the size of the constituent mrds. */
+	mrd = (struct ffa_comp_mrd *) ((uint8_t *) orig +
+	      emad_array[0].comp_mrd_offset);
+
+	/* Check the calculated address is within the memory descriptor. */
+	if ((uintptr_t) mrd >= (uintptr_t)((uint8_t *) orig + desc_size)) {
+		return 0;
+	}
+	size += mrd->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	return size;
+}
+
+/**
+ * spmc_shm_convert_shmem_obj_from_v1_0 - Converts a given v1.0 memory object.
+ * @out_obj:	The shared memory object to populate the converted descriptor.
+ * @orig:	The shared memory object containing the v1.0 descriptor.
+ *
+ * Return: true if the conversion is successful else false.
+ */
+static bool
+spmc_shm_convert_shmem_obj_from_v1_0(struct spmc_shmem_obj *out_obj,
+				     struct spmc_shmem_obj *orig)
+{
+	struct ffa_mtd_v1_0 *mtd_orig = (struct ffa_mtd_v1_0 *) &orig->desc;
+	struct ffa_mtd *out = &out_obj->desc;
+	struct ffa_emad_v1_0 *emad_array_in;
+	struct ffa_emad_v1_0 *emad_array_out;
+	struct ffa_comp_mrd *mrd_in;
+	struct ffa_comp_mrd *mrd_out;
+
+	size_t mrd_in_offset;
+	size_t mrd_out_offset;
+	size_t mrd_size = 0;
+
+	/* Populate the new descriptor format from the v1.0 struct. */
+	out->sender_id = mtd_orig->sender_id;
+	out->memory_region_attributes = mtd_orig->memory_region_attributes;
+	out->flags = mtd_orig->flags;
+	out->handle = mtd_orig->handle;
+	out->tag = mtd_orig->tag;
+	out->emad_count = mtd_orig->emad_count;
+	out->emad_size = sizeof(struct ffa_emad_v1_0);
+
+	/*
+	 * We will locate the emad descriptors directly after the ffa_mtd
+	 * struct. This will be 8-byte aligned.
+	 */
+	out->emad_offset = sizeof(struct ffa_mtd);
+
+	emad_array_in = mtd_orig->emad;
+	emad_array_out = (struct ffa_emad_v1_0 *)
+			 ((uint8_t *) out + out->emad_offset);
+
+	/* Copy across the emad structs. */
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		memcpy(&emad_array_out[i], &emad_array_in[i],
+		       sizeof(struct ffa_emad_v1_0));
+	}
+
+	/* Place the mrd descriptors after the end of the emad descriptors.*/
+	mrd_in_offset = emad_array_in->comp_mrd_offset;
+	mrd_out_offset = out->emad_offset + (out->emad_size * out->emad_count);
+	mrd_out = (struct ffa_comp_mrd *) ((uint8_t *) out + mrd_out_offset);
+
+	/* Add the size of the composite memory region descriptor. */
+	mrd_size += sizeof(struct ffa_comp_mrd);
+
+	/* Find the mrd descriptor. */
+	mrd_in = (struct ffa_comp_mrd *) ((uint8_t *) mtd_orig + mrd_in_offset);
+
+	/* Add the size of the constituent memory region descriptors. */
+	mrd_size += mrd_in->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	/*
+	 * Update the offset in the emads by the delta between the input and
+	 * output addresses.
+	 */
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		emad_array_out[i].comp_mrd_offset =
+			emad_array_in[i].comp_mrd_offset +
+			(mrd_out_offset - mrd_in_offset);
+	}
+
+	/* Verify that we stay within bound of the memory descriptors. */
+	if ((uintptr_t)((uint8_t *) mrd_in + mrd_size) >
+	     (uintptr_t)((uint8_t *) mtd_orig + orig->desc_size) ||
+	    ((uintptr_t)((uint8_t *) mrd_out + mrd_size) >
+	     (uintptr_t)((uint8_t *) out + out_obj->desc_size))) {
+		ERROR("%s: Invalid mrd structure.\n", __func__);
+		return false;
+	}
+
+	/* Copy the mrd descriptors directly. */
+	memcpy(mrd_out, mrd_in, mrd_size);
+
+	return true;
+}
+
+/**
+ * spmc_shm_convert_mtd_to_v1_0 - Converts a given v1.1 memory object to
+ *                                v1.0 memory object.
+ * @out_obj:    The shared memory object to populate the v1.0 descriptor.
+ * @orig:       The shared memory object containing the v1.1 descriptor.
+ *
+ * Return: true if the conversion is successful else false.
+ */
+static bool
+spmc_shm_convert_mtd_to_v1_0(struct spmc_shmem_obj *out_obj,
+			     struct spmc_shmem_obj *orig)
+{
+	struct ffa_mtd *mtd_orig = &orig->desc;
+	struct ffa_mtd_v1_0 *out = (struct ffa_mtd_v1_0 *) &out_obj->desc;
+	struct ffa_emad_v1_0 *emad_in;
+	struct ffa_emad_v1_0 *emad_array_in;
+	struct ffa_emad_v1_0 *emad_array_out;
+	struct ffa_comp_mrd *mrd_in;
+	struct ffa_comp_mrd *mrd_out;
+
+	size_t mrd_in_offset;
+	size_t mrd_out_offset;
+	size_t emad_out_array_size;
+	size_t mrd_size = 0;
+
+	/* Populate the v1.0 descriptor format from the v1.1 struct. */
+	out->sender_id = mtd_orig->sender_id;
+	out->memory_region_attributes = mtd_orig->memory_region_attributes;
+	out->flags = mtd_orig->flags;
+	out->handle = mtd_orig->handle;
+	out->tag = mtd_orig->tag;
+	out->emad_count = mtd_orig->emad_count;
+
+	/* Determine the location of the emad array in both descriptors. */
+	emad_array_in = (struct ffa_emad_v1_0 *)
+			((uint8_t *) mtd_orig + mtd_orig->emad_offset);
+	emad_array_out = out->emad;
+
+	/* Copy across the emad structs. */
+	emad_in = emad_array_in;
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		memcpy(&emad_array_out[i], emad_in,
+		       sizeof(struct ffa_emad_v1_0));
+
+		emad_in +=  mtd_orig->emad_size;
+	}
+
+	/* Place the mrd descriptors after the end of the emad descriptors. */
+	emad_out_array_size = sizeof(struct ffa_emad_v1_0) * out->emad_count;
+
+	mrd_out_offset =  (uint8_t *) out->emad - (uint8_t *) out +
+			  emad_out_array_size;
+
+	mrd_out = (struct ffa_comp_mrd *) ((uint8_t *) out + mrd_out_offset);
+
+	mrd_in_offset = mtd_orig->emad_offset +
+			(mtd_orig->emad_size * mtd_orig->emad_count);
+
+	/* Add the size of the composite memory region descriptor. */
+	mrd_size += sizeof(struct ffa_comp_mrd);
+
+	/* Find the mrd descriptor. */
+	mrd_in = (struct ffa_comp_mrd *) ((uint8_t *) mtd_orig + mrd_in_offset);
+
+	/* Add the size of the constituent memory region descriptors. */
+	mrd_size += mrd_in->address_range_count * sizeof(struct ffa_cons_mrd);
+
+	/*
+	 * Update the offset in the emads by the delta between the input and
+	 * output addresses.
+	 */
+	emad_in = emad_array_in;
+
+	for (unsigned int i = 0U; i < out->emad_count; i++) {
+		emad_array_out[i].comp_mrd_offset = emad_in->comp_mrd_offset +
+						    (mrd_out_offset -
+						     mrd_in_offset);
+		emad_in +=  mtd_orig->emad_size;
+	}
+
+	/* Verify that we stay within bound of the memory descriptors. */
+	if ((uintptr_t)((uint8_t *) mrd_in + mrd_size) >
+	     (uintptr_t)((uint8_t *) mtd_orig + orig->desc_size) ||
+	    ((uintptr_t)((uint8_t *) mrd_out + mrd_size) >
+	     (uintptr_t)((uint8_t *) out + out_obj->desc_size))) {
+		ERROR("%s: Invalid mrd structure.\n", __func__);
+		return false;
+	}
+
+	/* Copy the mrd descriptors directly. */
+	memcpy(mrd_out, mrd_in, mrd_size);
+
+	return true;
+}
+
+/**
+ * spmc_populate_ffa_v1_0_descriptor - Converts a given v1.1 memory object to
+ *                                     the v1.0 format and populates the
+ *                                     provided buffer.
+ * @dst:	    Buffer to populate v1.0 ffa_memory_region_descriptor.
+ * @orig_obj:	    Object containing v1.1 ffa_memory_region_descriptor.
+ * @buf_size:	    Size of the buffer to populate.
+ * @offset:	    The offset of the converted descriptor to copy.
+ * @copy_size:	    Will be populated with the number of bytes copied.
+ * @out_desc_size:  Will be populated with the total size of the v1.0
+ *                  descriptor.
+ *
+ * Return: 0 if conversion and population succeeded.
+ * Note: This function invalidates the reference to @orig therefore
+ * `spmc_shmem_obj_lookup` must be called if further usage is required.
+ */
+static uint32_t
+spmc_populate_ffa_v1_0_descriptor(void *dst, struct spmc_shmem_obj *orig_obj,
+				 size_t buf_size, size_t offset,
+				 size_t *copy_size, size_t *v1_0_desc_size)
+{
+		struct spmc_shmem_obj *v1_0_obj;
+
+		/* Calculate the size that the v1.0 descriptor will require. */
+		*v1_0_desc_size = spmc_shm_get_v1_0_descriptor_size(
+					&orig_obj->desc, orig_obj->desc_size);
+
+		if (*v1_0_desc_size == 0) {
+			ERROR("%s: cannot determine size of descriptor.\n",
+			      __func__);
+			return FFA_ERROR_INVALID_PARAMETER;
+		}
+
+		/* Get a new obj to store the v1.0 descriptor. */
+		v1_0_obj = spmc_shmem_obj_alloc(&spmc_shmem_obj_state,
+						*v1_0_desc_size);
+
+		if (!v1_0_obj) {
+			return FFA_ERROR_NO_MEMORY;
+		}
+
+		/* Perform the conversion from v1.1 to v1.0. */
+		if (!spmc_shm_convert_mtd_to_v1_0(v1_0_obj, orig_obj)) {
+			spmc_shmem_obj_free(&spmc_shmem_obj_state, v1_0_obj);
+			return FFA_ERROR_INVALID_PARAMETER;
+		}
+
+		*copy_size = MIN(v1_0_obj->desc_size - offset, buf_size);
+		memcpy(dst, (uint8_t *) &v1_0_obj->desc + offset, *copy_size);
+
+		/*
+		 * We're finished with the v1.0 descriptor for now so free it.
+		 * Note that this will invalidate any references to the v1.1
+		 * descriptor.
+		 */
+		spmc_shmem_obj_free(&spmc_shmem_obj_state, v1_0_obj);
+
+		return 0;
+}
+
+/**
+ * spmc_shmem_check_obj - Check that counts in descriptor match overall size.
+ * @obj:	  Object containing ffa_memory_region_descriptor.
+ * @ffa_version:  FF-A version of the provided descriptor.
+ *
+ * Return: 0 if object is valid, -EINVAL if constituent_memory_region_descriptor
+ * offset or count is invalid.
+ */
+static int spmc_shmem_check_obj(struct spmc_shmem_obj *obj,
+				uint32_t ffa_version)
+{
+	uint32_t comp_mrd_offset = 0;
+
+	if (obj->desc.emad_count == 0U) {
+		WARN("%s: unsupported attribute desc count %u.\n",
+		     __func__, obj->desc.emad_count);
+		return -EINVAL;
+	}
+
+	for (size_t emad_num = 0; emad_num < obj->desc.emad_count; emad_num++) {
+		size_t size;
+		size_t count;
+		size_t expected_size;
+		size_t total_page_count;
+		size_t emad_size;
+		size_t desc_size;
+		size_t header_emad_size;
+		uint32_t offset;
+		struct ffa_comp_mrd *comp;
+		struct ffa_emad_v1_0 *emad;
+
+		emad = spmc_shmem_obj_get_emad(&obj->desc, emad_num,
+					       ffa_version, &emad_size);
+		if (emad == NULL) {
+			WARN("%s: invalid emad structure.\n", __func__);
+			return -EINVAL;
+		}
+
+		/*
+		 * Validate the calculated emad address resides within the
+		 * descriptor.
+		 */
+		if ((uintptr_t) emad >=
+		    (uintptr_t)((uint8_t *) &obj->desc + obj->desc_size)) {
+			WARN("Invalid emad access.\n");
+			return -EINVAL;
+		}
+
+		offset = emad->comp_mrd_offset;
+
+		if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+			desc_size =  sizeof(struct ffa_mtd_v1_0);
+		} else {
+			desc_size =  sizeof(struct ffa_mtd);
+		}
+
+		header_emad_size = desc_size +
+			(obj->desc.emad_count * emad_size);
+
+		if (offset < header_emad_size) {
+			WARN("%s: invalid object, offset %u < header + emad %zu\n",
+			     __func__, offset, header_emad_size);
+			return -EINVAL;
+		}
+
+		size = obj->desc_size;
+
+		if (offset > size) {
+			WARN("%s: invalid object, offset %u > total size %zu\n",
+			     __func__, offset, obj->desc_size);
+			return -EINVAL;
+		}
+		size -= offset;
+
+		if (size < sizeof(struct ffa_comp_mrd)) {
+			WARN("%s: invalid object, offset %u, total size %zu, no header space.\n",
+			     __func__, offset, obj->desc_size);
+			return -EINVAL;
+		}
+		size -= sizeof(struct ffa_comp_mrd);
+
+		count = size / sizeof(struct ffa_cons_mrd);
+
+		comp = spmc_shmem_obj_get_comp_mrd(obj, ffa_version);
+
+		if (comp == NULL) {
+			WARN("%s: invalid comp_mrd offset\n", __func__);
+			return -EINVAL;
+		}
+
+		if (comp->address_range_count != count) {
+			WARN("%s: invalid object, desc count %u != %zu\n",
+			     __func__, comp->address_range_count, count);
+			return -EINVAL;
+		}
+
+		expected_size = offset + sizeof(*comp) +
+				spmc_shmem_obj_ffa_constituent_size(obj,
+								    ffa_version);
+
+		if (expected_size != obj->desc_size) {
+			WARN("%s: invalid object, computed size %zu != size %zu\n",
+			       __func__, expected_size, obj->desc_size);
+			return -EINVAL;
+		}
+
+		if (obj->desc_filled < obj->desc_size) {
+			/*
+			 * The whole descriptor has not yet been received.
+			 * Skip final checks.
+			 */
+			return 0;
+		}
+
+		/*
+		 * The offset provided to the composite memory region descriptor
+		 * should be consistent across endpoint descriptors. Store the
+		 * first entry and compare against subsequent entries.
+		 */
+		if (comp_mrd_offset == 0) {
+			comp_mrd_offset = offset;
+		} else {
+			if (comp_mrd_offset != offset) {
+				ERROR("%s: mismatching offsets provided, %u != %u\n",
+				       __func__, offset, comp_mrd_offset);
+				return -EINVAL;
+			}
+		}
+
+		total_page_count = 0;
+
+		for (size_t i = 0; i < count; i++) {
+			total_page_count +=
+				comp->address_range_array[i].page_count;
+		}
+		if (comp->total_page_count != total_page_count) {
+			WARN("%s: invalid object, desc total_page_count %u != %zu\n",
+			     __func__, comp->total_page_count,
+			total_page_count);
+			return -EINVAL;
+		}
+	}
+	return 0;
+}
+
+/**
+ * spmc_shmem_check_state_obj - Check if the descriptor describes memory
+ *				regions that are currently involved with an
+ *				existing memory transactions. This implies that
+ *				the memory is not in a valid state for lending.
+ * @obj:    Object containing ffa_memory_region_descriptor.
+ *
+ * Return: 0 if object is valid, -EINVAL if invalid memory state.
+ */
+static int spmc_shmem_check_state_obj(struct spmc_shmem_obj *obj,
+				      uint32_t ffa_version)
+{
+	size_t obj_offset = 0;
+	struct spmc_shmem_obj *inflight_obj;
+
+	struct ffa_comp_mrd *other_mrd;
+	struct ffa_comp_mrd *requested_mrd = spmc_shmem_obj_get_comp_mrd(obj,
+								  ffa_version);
+
+	if (requested_mrd == NULL) {
+		return -EINVAL;
+	}
+
+	inflight_obj = spmc_shmem_obj_get_next(&spmc_shmem_obj_state,
+					       &obj_offset);
+
+	while (inflight_obj != NULL) {
+		/*
+		 * Don't compare the transaction to itself or to partially
+		 * transmitted descriptors.
+		 */
+		if ((obj->desc.handle != inflight_obj->desc.handle) &&
+		    (obj->desc_size == obj->desc_filled)) {
+			other_mrd = spmc_shmem_obj_get_comp_mrd(inflight_obj,
+								ffa_version);
+			if (other_mrd == NULL) {
+				return -EINVAL;
+			}
+			if (overlapping_memory_regions(requested_mrd,
+						       other_mrd)) {
+				return -EINVAL;
+			}
+		}
+
+		inflight_obj = spmc_shmem_obj_get_next(&spmc_shmem_obj_state,
+						       &obj_offset);
+	}
+	return 0;
+}
+
+static long spmc_ffa_fill_desc(struct mailbox *mbox,
+			       struct spmc_shmem_obj *obj,
+			       uint32_t fragment_length,
+			       ffa_mtd_flag32_t mtd_flag,
+			       uint32_t ffa_version,
+			       void *smc_handle)
+{
+	int ret;
+	size_t emad_size;
+	uint32_t handle_low;
+	uint32_t handle_high;
+	struct ffa_emad_v1_0 *emad;
+	struct ffa_emad_v1_0 *other_emad;
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	if (fragment_length > mbox->rxtx_page_count * PAGE_SIZE_4KB) {
+		WARN("%s: bad fragment size %u > %u buffer size\n", __func__,
+		     fragment_length, mbox->rxtx_page_count * PAGE_SIZE_4KB);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	memcpy((uint8_t *)&obj->desc + obj->desc_filled,
+	       (uint8_t *) mbox->tx_buffer, fragment_length);
+
+	if (fragment_length > obj->desc_size - obj->desc_filled) {
+		WARN("%s: bad fragment size %u > %zu remaining\n", __func__,
+		     fragment_length, obj->desc_size - obj->desc_filled);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	/* Ensure that the sender ID resides in the normal world. */
+	if (ffa_is_secure_world_id(obj->desc.sender_id)) {
+		WARN("%s: Invalid sender ID 0x%x.\n",
+		     __func__, obj->desc.sender_id);
+		ret = FFA_ERROR_DENIED;
+		goto err_arg;
+	}
+
+	/* Ensure the NS bit is set to 0. */
+	if ((obj->desc.memory_region_attributes & FFA_MEM_ATTR_NS_BIT) != 0U) {
+		WARN("%s: NS mem attributes flags MBZ.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	/*
+	 * We don't currently support any optional flags so ensure none are
+	 * requested.
+	 */
+	if (obj->desc.flags != 0U && mtd_flag != 0U &&
+	    (obj->desc.flags != mtd_flag)) {
+		WARN("%s: invalid memory transaction flags %u != %u\n",
+		     __func__, obj->desc.flags, mtd_flag);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_arg;
+	}
+
+	if (obj->desc_filled == 0U) {
+		/* First fragment, descriptor header has been copied */
+		obj->desc.handle = spmc_shmem_obj_state.next_handle++;
+		obj->desc.flags |= mtd_flag;
+	}
+
+	obj->desc_filled += fragment_length;
+	ret = spmc_shmem_check_obj(obj, ffa_version);
+	if (ret != 0) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_bad_desc;
+	}
+
+	handle_low = (uint32_t)obj->desc.handle;
+	handle_high = obj->desc.handle >> 32;
+
+	if (obj->desc_filled != obj->desc_size) {
+		SMC_RET8(smc_handle, FFA_MEM_FRAG_RX, handle_low,
+			 handle_high, obj->desc_filled,
+			 (uint32_t)obj->desc.sender_id << 16, 0, 0, 0);
+	}
+
+	/* The full descriptor has been received, perform any final checks. */
+
+	/*
+	 * If a partition ID resides in the secure world validate that the
+	 * partition ID is for a known partition. Ignore any partition ID
+	 * belonging to the normal world as it is assumed the Hypervisor will
+	 * have validated these.
+	 */
+	for (size_t i = 0; i < obj->desc.emad_count; i++) {
+		emad = spmc_shmem_obj_get_emad(&obj->desc, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_bad_desc;
+		}
+
+		ffa_endpoint_id16_t ep_id = emad->mapd.endpoint_id;
+
+		if (ffa_is_secure_world_id(ep_id)) {
+			if (spmc_get_sp_ctx(ep_id) == NULL) {
+				WARN("%s: Invalid receiver id 0x%x\n",
+				     __func__, ep_id);
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_bad_desc;
+			}
+		}
+	}
+
+	/* Ensure partition IDs are not duplicated. */
+	for (size_t i = 0; i < obj->desc.emad_count; i++) {
+		emad = spmc_shmem_obj_get_emad(&obj->desc, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_bad_desc;
+		}
+		for (size_t j = i + 1; j < obj->desc.emad_count; j++) {
+			other_emad = spmc_shmem_obj_get_emad(&obj->desc, j,
+							     ffa_version,
+							     &emad_size);
+			if (other_emad == NULL) {
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_bad_desc;
+			}
+
+			if (emad->mapd.endpoint_id ==
+				other_emad->mapd.endpoint_id) {
+				WARN("%s: Duplicated endpoint id 0x%x\n",
+				     __func__, emad->mapd.endpoint_id);
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_bad_desc;
+			}
+		}
+	}
+
+	ret = spmc_shmem_check_state_obj(obj, ffa_version);
+	if (ret) {
+		ERROR("%s: invalid memory region descriptor.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_bad_desc;
+	}
+
+	/*
+	 * Everything checks out, if the sender was using FF-A v1.0, convert
+	 * the descriptor format to use the v1.1 structures.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		struct spmc_shmem_obj *v1_1_obj;
+		uint64_t mem_handle;
+
+		/* Calculate the size that the v1.1 descriptor will required. */
+		size_t v1_1_desc_size =
+		    spmc_shm_get_v1_1_descriptor_size((void *) &obj->desc,
+						      fragment_length);
+
+		if (v1_1_desc_size == 0U) {
+			ERROR("%s: cannot determine size of descriptor.\n",
+			      __func__);
+			goto err_arg;
+		}
+
+		/* Get a new obj to store the v1.1 descriptor. */
+		v1_1_obj =
+		    spmc_shmem_obj_alloc(&spmc_shmem_obj_state, v1_1_desc_size);
+
+		if (!obj) {
+			ret = FFA_ERROR_NO_MEMORY;
+			goto err_arg;
+		}
+
+		/* Perform the conversion from v1.0 to v1.1. */
+		v1_1_obj->desc_size = v1_1_desc_size;
+		v1_1_obj->desc_filled = v1_1_desc_size;
+		if (!spmc_shm_convert_shmem_obj_from_v1_0(v1_1_obj, obj)) {
+			ERROR("%s: Could not convert mtd!\n", __func__);
+			spmc_shmem_obj_free(&spmc_shmem_obj_state, v1_1_obj);
+			goto err_arg;
+		}
+
+		/*
+		 * We're finished with the v1.0 descriptor so free it
+		 * and continue our checks with the new v1.1 descriptor.
+		 */
+		mem_handle = obj->desc.handle;
+		spmc_shmem_obj_free(&spmc_shmem_obj_state, obj);
+		obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+		if (obj == NULL) {
+			ERROR("%s: Failed to find converted descriptor.\n",
+			     __func__);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			return spmc_ffa_error_return(smc_handle, ret);
+		}
+	}
+
+	/* Allow for platform specific operations to be performed. */
+	ret = plat_spmc_shmem_begin(&obj->desc);
+	if (ret != 0) {
+		goto err_arg;
+	}
+
+	SMC_RET8(smc_handle, FFA_SUCCESS_SMC32, 0, handle_low, handle_high, 0,
+		 0, 0, 0);
+
+err_bad_desc:
+err_arg:
+	spmc_shmem_obj_free(&spmc_shmem_obj_state, obj);
+	return spmc_ffa_error_return(smc_handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_send - FFA_MEM_SHARE/LEND implementation.
+ * @client:             Client state.
+ * @total_length:       Total length of shared memory descriptor.
+ * @fragment_length:    Length of fragment of shared memory descriptor passed in
+ *                      this call.
+ * @address:            Not supported, must be 0.
+ * @page_count:         Not supported, must be 0.
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_FRAG_RX or SMC_FC_FFA_SUCCESS.
+ *
+ * Implements a subset of the FF-A FFA_MEM_SHARE and FFA_MEM_LEND calls needed
+ * to share or lend memory from non-secure os to secure os (with no stream
+ * endpoints).
+ *
+ * Return: 0 on success, error code on failure.
+ */
+long spmc_ffa_mem_send(uint32_t smc_fid,
+			bool secure_origin,
+			uint64_t total_length,
+			uint32_t fragment_length,
+			uint64_t address,
+			uint32_t page_count,
+			void *cookie,
+			void *handle,
+			uint64_t flags)
+
+{
+	long ret;
+	struct spmc_shmem_obj *obj;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	ffa_mtd_flag32_t mtd_flag;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+
+	if (address != 0U || page_count != 0U) {
+		WARN("%s: custom memory region for message not supported.\n",
+		     __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if (secure_origin) {
+		WARN("%s: unsupported share direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/*
+	 * Check if the descriptor is smaller than the v1.0 descriptor. The
+	 * descriptor cannot be smaller than this structure.
+	 */
+	if (fragment_length < sizeof(struct ffa_mtd_v1_0)) {
+		WARN("%s: bad first fragment size %u < %zu\n",
+		     __func__, fragment_length, sizeof(struct ffa_mtd_v1_0));
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if ((smc_fid & FUNCID_NUM_MASK) == FFA_FNUM_MEM_SHARE) {
+		mtd_flag = FFA_MTD_FLAG_TYPE_SHARE_MEMORY;
+	} else if ((smc_fid & FUNCID_NUM_MASK) == FFA_FNUM_MEM_LEND) {
+		mtd_flag = FFA_MTD_FLAG_TYPE_LEND_MEMORY;
+	} else {
+		WARN("%s: invalid memory management operation.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+	obj = spmc_shmem_obj_alloc(&spmc_shmem_obj_state, total_length);
+	if (obj == NULL) {
+		ret = FFA_ERROR_NO_MEMORY;
+		goto err_unlock;
+	}
+
+	spin_lock(&mbox->lock);
+	ret = spmc_ffa_fill_desc(mbox, obj, fragment_length, mtd_flag,
+				 ffa_version, handle);
+	spin_unlock(&mbox->lock);
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return ret;
+
+err_unlock:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_frag_tx - FFA_MEM_FRAG_TX implementation.
+ * @client:             Client state.
+ * @handle_low:         Handle_low value returned from FFA_MEM_FRAG_RX.
+ * @handle_high:        Handle_high value returned from FFA_MEM_FRAG_RX.
+ * @fragment_length:    Length of fragments transmitted.
+ * @sender_id:          Vmid of sender in bits [31:16]
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_FRAG_RX or SMC_FC_FFA_SUCCESS.
+ *
+ * Return: @smc_handle on success, error code on failure.
+ */
+long spmc_ffa_mem_frag_tx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint64_t handle_low,
+			  uint64_t handle_high,
+			  uint32_t fragment_length,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	long ret;
+	uint32_t desc_sender_id;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+
+	struct spmc_shmem_obj *obj;
+	uint64_t mem_handle = handle_low | (((uint64_t)handle_high) << 32);
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+	if (obj == NULL) {
+		WARN("%s: invalid handle, 0x%lx, not a valid handle.\n",
+		     __func__, mem_handle);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+
+	desc_sender_id = (uint32_t)obj->desc.sender_id << 16;
+	if (sender_id != desc_sender_id) {
+		WARN("%s: invalid sender_id 0x%x != 0x%x\n", __func__,
+		     sender_id, desc_sender_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+
+	if (obj->desc_filled == obj->desc_size) {
+		WARN("%s: object desc already filled, %zu\n", __func__,
+		     obj->desc_filled);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+
+	spin_lock(&mbox->lock);
+	ret = spmc_ffa_fill_desc(mbox, obj, fragment_length, 0, ffa_version,
+				 handle);
+	spin_unlock(&mbox->lock);
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return ret;
+
+err_unlock:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_retrieve_set_ns_bit - Set the NS bit in the response descriptor
+ *				      if the caller implements a version greater
+ *				      than FF-A 1.0 or if they have requested
+ *				      the functionality.
+ *				      TODO: We are assuming that the caller is
+ *				      an SP. To support retrieval from the
+ *				      normal world this function will need to be
+ *				      expanded accordingly.
+ * @resp:       Descriptor populated in callers RX buffer.
+ * @sp_ctx:     Context of the calling SP.
+ */
+void spmc_ffa_mem_retrieve_set_ns_bit(struct ffa_mtd *resp,
+			 struct secure_partition_desc *sp_ctx)
+{
+	if (sp_ctx->ffa_version > MAKE_FFA_VERSION(1, 0) ||
+	    sp_ctx->ns_bit_requested) {
+		/*
+		 * Currently memory senders must reside in the normal
+		 * world, and we do not have the functionlaity to change
+		 * the state of memory dynamically. Therefore we can always set
+		 * the NS bit to 1.
+		 */
+		resp->memory_region_attributes |= FFA_MEM_ATTR_NS_BIT;
+	}
+}
+
+/**
+ * spmc_ffa_mem_retrieve_req - FFA_MEM_RETRIEVE_REQ implementation.
+ * @smc_fid:            FID of SMC
+ * @total_length:       Total length of retrieve request descriptor if this is
+ *                      the first call. Otherwise (unsupported) must be 0.
+ * @fragment_length:    Length of fragment of retrieve request descriptor passed
+ *                      in this call. Only @fragment_length == @length is
+ *                      supported by this implementation.
+ * @address:            Not supported, must be 0.
+ * @page_count:         Not supported, must be 0.
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_RETRIEVE_RESP.
+ *
+ * Implements a subset of the FF-A FFA_MEM_RETRIEVE_REQ call.
+ * Used by secure os to retrieve memory already shared by non-secure os.
+ * If the data does not fit in a single FFA_MEM_RETRIEVE_RESP message,
+ * the client must call FFA_MEM_FRAG_RX until the full response has been
+ * received.
+ *
+ * Return: @handle on success, error code on failure.
+ */
+long
+spmc_ffa_mem_retrieve_req(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint32_t total_length,
+			  uint32_t fragment_length,
+			  uint64_t address,
+			  uint32_t page_count,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	int ret;
+	size_t buf_size;
+	size_t copy_size = 0;
+	size_t min_desc_size;
+	size_t out_desc_size = 0;
+
+	/*
+	 * Currently we are only accessing fields that are the same in both the
+	 * v1.0 and v1.1 mtd struct therefore we can use a v1.1 struct directly
+	 * here. We only need validate against the appropriate struct size.
+	 */
+	struct ffa_mtd *resp;
+	const struct ffa_mtd *req;
+	struct spmc_shmem_obj *obj = NULL;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+	struct secure_partition_desc *sp_ctx = spmc_get_current_sp_ctx();
+
+	if (!secure_origin) {
+		WARN("%s: unsupported retrieve req direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if (address != 0U || page_count != 0U) {
+		WARN("%s: custom memory region not supported.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&mbox->lock);
+
+	req = mbox->tx_buffer;
+	resp = mbox->rx_buffer;
+	buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	if (mbox->state != MAILBOX_STATE_EMPTY) {
+		WARN("%s: RX Buffer is full! %d\n", __func__, mbox->state);
+		ret = FFA_ERROR_DENIED;
+		goto err_unlock_mailbox;
+	}
+
+	if (fragment_length != total_length) {
+		WARN("%s: fragmented retrieve request not supported.\n",
+		     __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	if (req->emad_count == 0U) {
+		WARN("%s: unsupported attribute desc count %u.\n",
+		     __func__, obj->desc.emad_count);
+		return -EINVAL;
+	}
+
+	/* Determine the appropriate minimum descriptor size. */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		min_desc_size = sizeof(struct ffa_mtd_v1_0);
+	} else {
+		min_desc_size = sizeof(struct ffa_mtd);
+	}
+	if (total_length < min_desc_size) {
+		WARN("%s: invalid length %u < %zu\n", __func__, total_length,
+		     min_desc_size);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, req->handle);
+	if (obj == NULL) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (obj->desc_filled != obj->desc_size) {
+		WARN("%s: incomplete object desc filled %zu < size %zu\n",
+		     __func__, obj->desc_filled, obj->desc_size);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->emad_count != 0U && req->sender_id != obj->desc.sender_id) {
+		WARN("%s: wrong sender id 0x%x != 0x%x\n",
+		     __func__, req->sender_id, obj->desc.sender_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->emad_count != 0U && req->tag != obj->desc.tag) {
+		WARN("%s: wrong tag 0x%lx != 0x%lx\n",
+		     __func__, req->tag, obj->desc.tag);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->emad_count != 0U && req->emad_count != obj->desc.emad_count) {
+		WARN("%s: mistmatch of endpoint counts %u != %u\n",
+		     __func__, req->emad_count, obj->desc.emad_count);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	/* Ensure the NS bit is set to 0 in the request. */
+	if ((req->memory_region_attributes & FFA_MEM_ATTR_NS_BIT) != 0U) {
+		WARN("%s: NS mem attributes flags MBZ.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (req->flags != 0U) {
+		if ((req->flags & FFA_MTD_FLAG_TYPE_MASK) !=
+		    (obj->desc.flags & FFA_MTD_FLAG_TYPE_MASK)) {
+			/*
+			 * If the retrieve request specifies the memory
+			 * transaction ensure it matches what we expect.
+			 */
+			WARN("%s: wrong mem transaction flags %x != %x\n",
+			__func__, req->flags, obj->desc.flags);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+
+		if (req->flags != FFA_MTD_FLAG_TYPE_SHARE_MEMORY &&
+		    req->flags != FFA_MTD_FLAG_TYPE_LEND_MEMORY) {
+			/*
+			 * Current implementation does not support donate and
+			 * it supports no other flags.
+			 */
+			WARN("%s: invalid flags 0x%x\n", __func__, req->flags);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	/* Validate that the provided emad offset and structure is valid.*/
+	for (size_t i = 0; i < req->emad_count; i++) {
+		size_t emad_size;
+		struct ffa_emad_v1_0 *emad;
+
+		emad = spmc_shmem_obj_get_emad(req, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			WARN("%s: invalid emad structure.\n", __func__);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+
+		if ((uintptr_t) emad >= (uintptr_t)
+					((uint8_t *) req + total_length)) {
+			WARN("Invalid emad access.\n");
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	/*
+	 * Validate all the endpoints match in the case of multiple
+	 * borrowers. We don't mandate that the order of the borrowers
+	 * must match in the descriptors therefore check to see if the
+	 * endpoints match in any order.
+	 */
+	for (size_t i = 0; i < req->emad_count; i++) {
+		bool found = false;
+		size_t emad_size;
+		struct ffa_emad_v1_0 *emad;
+		struct ffa_emad_v1_0 *other_emad;
+
+		emad = spmc_shmem_obj_get_emad(req, i, ffa_version,
+					       &emad_size);
+		if (emad == NULL) {
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+
+		for (size_t j = 0; j < obj->desc.emad_count; j++) {
+			other_emad = spmc_shmem_obj_get_emad(
+					&obj->desc, j, MAKE_FFA_VERSION(1, 1),
+					&emad_size);
+
+			if (other_emad == NULL) {
+				ret = FFA_ERROR_INVALID_PARAMETER;
+				goto err_unlock_all;
+			}
+
+			if (req->emad_count &&
+			    emad->mapd.endpoint_id ==
+			    other_emad->mapd.endpoint_id) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			WARN("%s: invalid receiver id (0x%x).\n",
+			     __func__, emad->mapd.endpoint_id);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	mbox->state = MAILBOX_STATE_FULL;
+
+	if (req->emad_count != 0U) {
+		obj->in_use++;
+	}
+
+	/*
+	 * If the caller is v1.0 convert the descriptor, otherwise copy
+	 * directly.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		ret = spmc_populate_ffa_v1_0_descriptor(resp, obj, buf_size, 0,
+							&copy_size,
+							&out_desc_size);
+		if (ret != 0U) {
+			ERROR("%s: Failed to process descriptor.\n", __func__);
+			goto err_unlock_all;
+		}
+	} else {
+		copy_size = MIN(obj->desc_size, buf_size);
+		out_desc_size = obj->desc_size;
+
+		memcpy(resp, &obj->desc, copy_size);
+	}
+
+	/* Set the NS bit in the response if applicable. */
+	spmc_ffa_mem_retrieve_set_ns_bit(resp, sp_ctx);
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	spin_unlock(&mbox->lock);
+
+	SMC_RET8(handle, FFA_MEM_RETRIEVE_RESP, out_desc_size,
+		 copy_size, 0, 0, 0, 0, 0);
+
+err_unlock_all:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+err_unlock_mailbox:
+	spin_unlock(&mbox->lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_frag_rx - FFA_MEM_FRAG_RX implementation.
+ * @client:             Client state.
+ * @handle_low:         Handle passed to &FFA_MEM_RETRIEVE_REQ. Bit[31:0].
+ * @handle_high:        Handle passed to &FFA_MEM_RETRIEVE_REQ. Bit[63:32].
+ * @fragment_offset:    Byte offset in descriptor to resume at.
+ * @sender_id:          Bit[31:16]: Endpoint id of sender if client is a
+ *                      hypervisor. 0 otherwise.
+ * @smc_handle:         Handle passed to smc call. Used to return
+ *                      FFA_MEM_FRAG_TX.
+ *
+ * Return: @smc_handle on success, error code on failure.
+ */
+long spmc_ffa_mem_frag_rx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint32_t handle_low,
+			  uint32_t handle_high,
+			  uint32_t fragment_offset,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags)
+{
+	int ret;
+	void *src;
+	size_t buf_size;
+	size_t copy_size;
+	size_t full_copy_size;
+	uint32_t desc_sender_id;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	uint64_t mem_handle = handle_low | (((uint64_t)handle_high) << 32);
+	struct spmc_shmem_obj *obj;
+	uint32_t ffa_version = get_partition_ffa_version(secure_origin);
+
+	if (!secure_origin) {
+		WARN("%s: can only be called from swld.\n",
+		     __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+	if (obj == NULL) {
+		WARN("%s: invalid handle, 0x%lx, not a valid handle.\n",
+		     __func__, mem_handle);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_shmem;
+	}
+
+	desc_sender_id = (uint32_t)obj->desc.sender_id << 16;
+	if (sender_id != 0U && sender_id != desc_sender_id) {
+		WARN("%s: invalid sender_id 0x%x != 0x%x\n", __func__,
+		     sender_id, desc_sender_id);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_shmem;
+	}
+
+	if (fragment_offset >= obj->desc_size) {
+		WARN("%s: invalid fragment_offset 0x%x >= 0x%zx\n",
+		     __func__, fragment_offset, obj->desc_size);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_shmem;
+	}
+
+	spin_lock(&mbox->lock);
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (mbox->state != MAILBOX_STATE_EMPTY) {
+		WARN("%s: RX Buffer is full!\n", __func__);
+		ret = FFA_ERROR_DENIED;
+		goto err_unlock_all;
+	}
+
+	buf_size = mbox->rxtx_page_count * FFA_PAGE_SIZE;
+
+	mbox->state = MAILBOX_STATE_FULL;
+
+	/*
+	 * If the caller is v1.0 convert the descriptor, otherwise copy
+	 * directly.
+	 */
+	if (ffa_version == MAKE_FFA_VERSION(1, 0)) {
+		size_t out_desc_size;
+
+		ret = spmc_populate_ffa_v1_0_descriptor(mbox->rx_buffer, obj,
+							buf_size,
+							fragment_offset,
+							&copy_size,
+							&out_desc_size);
+		if (ret != 0U) {
+			ERROR("%s: Failed to process descriptor.\n", __func__);
+			goto err_unlock_all;
+		}
+	} else {
+		full_copy_size = obj->desc_size - fragment_offset;
+		copy_size = MIN(full_copy_size, buf_size);
+
+		src = &obj->desc;
+
+		memcpy(mbox->rx_buffer, src + fragment_offset, copy_size);
+	}
+
+	spin_unlock(&mbox->lock);
+	spin_unlock(&spmc_shmem_obj_state.lock);
+
+	SMC_RET8(handle, FFA_MEM_FRAG_TX, handle_low, handle_high,
+		 copy_size, sender_id, 0, 0, 0);
+
+err_unlock_all:
+	spin_unlock(&mbox->lock);
+err_unlock_shmem:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_relinquish - FFA_MEM_RELINQUISH implementation.
+ * @client:             Client state.
+ *
+ * Implements a subset of the FF-A FFA_MEM_RELINQUISH call.
+ * Used by secure os release previously shared memory to non-secure os.
+ *
+ * The handle to release must be in the client's (secure os's) transmit buffer.
+ *
+ * Return: 0 on success, error code on failure.
+ */
+int spmc_ffa_mem_relinquish(uint32_t smc_fid,
+			    bool secure_origin,
+			    uint32_t handle_low,
+			    uint32_t handle_high,
+			    uint32_t fragment_offset,
+			    uint32_t sender_id,
+			    void *cookie,
+			    void *handle,
+			    uint64_t flags)
+{
+	int ret;
+	struct mailbox *mbox = spmc_get_mbox_desc(secure_origin);
+	struct spmc_shmem_obj *obj;
+	const struct ffa_mem_relinquish_descriptor *req;
+
+	if (!secure_origin) {
+		WARN("%s: unsupported relinquish direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&mbox->lock);
+
+	if (mbox->rxtx_page_count == 0U) {
+		WARN("%s: buffer pair not registered.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	req = mbox->tx_buffer;
+
+	if (req->flags != 0U) {
+		WARN("%s: unsupported flags 0x%x\n", __func__, req->flags);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	if (req->endpoint_count == 0) {
+		WARN("%s: endpoint count cannot be 0.\n", __func__);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_mailbox;
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, req->handle);
+	if (obj == NULL) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	if (obj->desc.emad_count != req->endpoint_count) {
+		WARN("%s: mismatch of endpoint count %u != %u\n", __func__,
+		     obj->desc.emad_count, req->endpoint_count);
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+
+	/* Validate requested endpoint IDs match descriptor. */
+	for (size_t i = 0; i < req->endpoint_count; i++) {
+		bool found = false;
+		size_t emad_size;
+		struct ffa_emad_v1_0 *emad;
+
+		for (unsigned int j = 0; j < obj->desc.emad_count; j++) {
+			emad = spmc_shmem_obj_get_emad(&obj->desc, j,
+							MAKE_FFA_VERSION(1, 1),
+							&emad_size);
+			if (req->endpoint_array[i] ==
+			    emad->mapd.endpoint_id) {
+				found = true;
+				break;
+			}
+		}
+
+		if (!found) {
+			WARN("%s: Invalid endpoint ID (0x%x).\n",
+			     __func__, req->endpoint_array[i]);
+			ret = FFA_ERROR_INVALID_PARAMETER;
+			goto err_unlock_all;
+		}
+	}
+
+	if (obj->in_use == 0U) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock_all;
+	}
+	obj->in_use--;
+
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	spin_unlock(&mbox->lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+
+err_unlock_all:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+err_unlock_mailbox:
+	spin_unlock(&mbox->lock);
+	return spmc_ffa_error_return(handle, ret);
+}
+
+/**
+ * spmc_ffa_mem_reclaim - FFA_MEM_RECLAIM implementation.
+ * @client:         Client state.
+ * @handle_low:     Unique handle of shared memory object to reclaim. Bit[31:0].
+ * @handle_high:    Unique handle of shared memory object to reclaim.
+ *                  Bit[63:32].
+ * @flags:          Unsupported, ignored.
+ *
+ * Implements a subset of the FF-A FFA_MEM_RECLAIM call.
+ * Used by non-secure os reclaim memory previously shared with secure os.
+ *
+ * Return: 0 on success, error code on failure.
+ */
+int spmc_ffa_mem_reclaim(uint32_t smc_fid,
+			 bool secure_origin,
+			 uint32_t handle_low,
+			 uint32_t handle_high,
+			 uint32_t mem_flags,
+			 uint64_t x4,
+			 void *cookie,
+			 void *handle,
+			 uint64_t flags)
+{
+	int ret;
+	struct spmc_shmem_obj *obj;
+	uint64_t mem_handle = handle_low | (((uint64_t)handle_high) << 32);
+
+	if (secure_origin) {
+		WARN("%s: unsupported reclaim direction.\n", __func__);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	if (mem_flags != 0U) {
+		WARN("%s: unsupported flags 0x%x\n", __func__, mem_flags);
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	spin_lock(&spmc_shmem_obj_state.lock);
+
+	obj = spmc_shmem_obj_lookup(&spmc_shmem_obj_state, mem_handle);
+	if (obj == NULL) {
+		ret = FFA_ERROR_INVALID_PARAMETER;
+		goto err_unlock;
+	}
+	if (obj->in_use != 0U) {
+		ret = FFA_ERROR_DENIED;
+		goto err_unlock;
+	}
+
+	/* Allow for platform specific operations to be performed. */
+	ret = plat_spmc_shmem_reclaim(&obj->desc);
+	if (ret != 0) {
+		goto err_unlock;
+	}
+
+	spmc_shmem_obj_free(&spmc_shmem_obj_state, obj);
+	spin_unlock(&spmc_shmem_obj_state.lock);
+
+	SMC_RET1(handle, FFA_SUCCESS_SMC32);
+
+err_unlock:
+	spin_unlock(&spmc_shmem_obj_state.lock);
+	return spmc_ffa_error_return(handle, ret);
+}
diff --git a/services/std_svc/spm/el3_spmc/spmc_shared_mem.h b/services/std_svc/spm/el3_spmc/spmc_shared_mem.h
new file mode 100644
index 0000000..839f7a1
--- /dev/null
+++ b/services/std_svc/spm/el3_spmc/spmc_shared_mem.h
@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SPMC_SHARED_MEM_H
+#define SPMC_SHARED_MEM_H
+
+#include <services/el3_spmc_ffa_memory.h>
+
+/**
+ * struct ffa_mem_relinquish_descriptor - Relinquish request descriptor.
+ * @handle:
+ *         Id of shared memory object to relinquish.
+ * @flags:
+ *         If bit 0 is set clear memory after unmapping from borrower. Must be 0
+ *         for share. Bit[1]: Time slicing. Not supported, must be 0. All other
+ *         bits are reserved 0.
+ * @endpoint_count:
+ *         Number of entries in @endpoint_array.
+ * @endpoint_array:
+ *         Array of endpoint ids.
+ */
+struct ffa_mem_relinquish_descriptor {
+	uint64_t handle;
+	uint32_t flags;
+	uint32_t endpoint_count;
+	ffa_endpoint_id16_t endpoint_array[];
+};
+CASSERT(sizeof(struct ffa_mem_relinquish_descriptor) == 16,
+	assert_ffa_mem_relinquish_descriptor_size_mismatch);
+
+/**
+ * struct spmc_shmem_obj_state - Global state.
+ * @data:           Backing store for spmc_shmem_obj objects.
+ * @data_size:      The size allocated for the backing store.
+ * @allocated:      Number of bytes allocated in @data.
+ * @next_handle:    Handle used for next allocated object.
+ * @lock:           Lock protecting all state in this file.
+ */
+struct spmc_shmem_obj_state {
+	uint8_t *data;
+	size_t data_size;
+	size_t allocated;
+	uint64_t next_handle;
+	spinlock_t lock;
+};
+
+extern struct spmc_shmem_obj_state spmc_shmem_obj_state;
+extern int plat_spmc_shmem_begin(struct ffa_mtd *desc);
+extern int plat_spmc_shmem_reclaim(struct ffa_mtd *desc);
+
+long spmc_ffa_mem_send(uint32_t smc_fid,
+		       bool secure_origin,
+		       uint64_t total_length,
+		       uint32_t fragment_length,
+		       uint64_t address,
+		       uint32_t page_count,
+		       void *cookie,
+		       void *handle,
+		       uint64_t flags);
+
+long spmc_ffa_mem_frag_tx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint64_t handle_low,
+			  uint64_t handle_high,
+			  uint32_t fragment_length,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags);
+
+long spmc_ffa_mem_retrieve_req(uint32_t smc_fid,
+			       bool secure_origin,
+			       uint32_t total_length,
+			       uint32_t fragment_length,
+			       uint64_t address,
+			       uint32_t page_count,
+			       void *cookie,
+			       void *handle,
+			       uint64_t flags);
+
+long spmc_ffa_mem_frag_rx(uint32_t smc_fid,
+			  bool secure_origin,
+			  uint32_t handle_low,
+			  uint32_t handle_high,
+			  uint32_t fragment_offset,
+			  uint32_t sender_id,
+			  void *cookie,
+			  void *handle,
+			  uint64_t flags);
+
+
+int spmc_ffa_mem_relinquish(uint32_t smc_fid,
+			    bool secure_origin,
+			    uint32_t handle_low,
+			    uint32_t handle_high,
+			    uint32_t fragment_offset,
+			    uint32_t sender_id,
+			    void *cookie,
+			    void *handle,
+			    uint64_t flags);
+
+int spmc_ffa_mem_reclaim(uint32_t smc_fid,
+			 bool secure_origin,
+			 uint32_t handle_low,
+			 uint32_t handle_high,
+			 uint32_t mem_flags,
+			 uint64_t x4,
+			 void *cookie,
+			 void *handle,
+			 uint64_t flags);
+
+#endif /* SPMC_SHARED_MEM_H */
diff --git a/services/std_svc/spmd/spmd_main.c b/services/std_svc/spmd/spmd_main.c
index 777a962..e388784 100644
--- a/services/std_svc/spmd/spmd_main.c
+++ b/services/std_svc/spmd/spmd_main.c
@@ -684,7 +684,7 @@
 			    (SMC_GET_GP(gpregs, CTX_GPREG_X0) !=
 				FFA_MSG_SEND_DIRECT_RESP_SMC32) ||
 			    (SMC_GET_GP(gpregs, CTX_GPREG_X2) !=
-				(SPMD_FWK_MSG_BIT |
+				(FFA_FWK_MSG_BIT |
 				 SPMD_FWK_MSG_FFA_VERSION_RESP))) {
 				ERROR("Failed to forward FFA_VERSION\n");
 				ret = FFA_ERROR_NOT_SUPPORTED;
@@ -875,6 +875,8 @@
 	case FFA_MEM_RETRIEVE_RESP:
 	case FFA_MEM_RELINQUISH:
 	case FFA_MEM_RECLAIM:
+	case FFA_MEM_FRAG_TX:
+	case FFA_MEM_FRAG_RX:
 	case FFA_SUCCESS_SMC32:
 	case FFA_SUCCESS_SMC64:
 		/*
diff --git a/services/std_svc/spmd/spmd_pm.c b/services/std_svc/spmd/spmd_pm.c
index b719161..a2704dd 100644
--- a/services/std_svc/spmd/spmd_pm.c
+++ b/services/std_svc/spmd/spmd_pm.c
@@ -123,7 +123,7 @@
 
 	/* Build an SPMD to SPMC direct message request. */
 	spmd_build_spmc_message(get_gpregs_ctx(&ctx->cpu_ctx),
-				SPMD_FWK_MSG_PSCI, PSCI_CPU_OFF);
+				FFA_FWK_MSG_PSCI, PSCI_CPU_OFF);
 
 	rc = spmd_spm_core_sync_entry(ctx);
 	if (rc != 0ULL) {
diff --git a/services/std_svc/spmd/spmd_private.h b/services/std_svc/spmd/spmd_private.h
index 4c298c9..07fecb6 100644
--- a/services/std_svc/spmd/spmd_private.h
+++ b/services/std_svc/spmd/spmd_private.h
@@ -59,8 +59,6 @@
 #define FFA_NS_ENDPOINT_ID			U(0)
 
 /* 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)
 #define SPMD_FWK_MSG_FFA_VERSION_REQ		U(0x8)
 #define SPMD_FWK_MSG_FFA_VERSION_RESP		U(0x9)
 
diff --git a/tools/cert_create/Makefile b/tools/cert_create/Makefile
index 77d2007..ca548b8 100644
--- a/tools/cert_create/Makefile
+++ b/tools/cert_create/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -62,7 +62,14 @@
 # Make soft links and include from local directory otherwise wrong headers
 # could get pulled in from firmware tree.
 INC_DIR += -I ./include -I ${PLAT_INCLUDE} -I ${OPENSSL_DIR}/include
-LIB_DIR := -L ${OPENSSL_DIR}/lib
+
+# Include library directories where OpenSSL library files are located.
+# For a normal installation (i.e.: when ${OPENSSL_DIR} = /usr or
+# /usr/local), binaries are located under the ${OPENSSL_DIR}/lib/
+# directory. However, for a local build of OpenSSL, the built binaries are
+# located under the main project directory (i.e.: ${OPENSSL_DIR}, not
+# ${OPENSSL_DIR}/lib/).
+LIB_DIR := -L ${OPENSSL_DIR}/lib -L ${OPENSSL_DIR}
 LIB := -lssl -lcrypto
 
 HOSTCC ?= gcc
diff --git a/tools/cert_create/src/cert.c b/tools/cert_create/src/cert.c
index 4b35d73..67ae1d6 100644
--- a/tools/cert_create/src/cert.c
+++ b/tools/cert_create/src/cert.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -39,7 +39,7 @@
 	if (!btmp)
 		return 0;
 
-	if (!BN_pseudo_rand(btmp, SERIAL_RAND_BITS, 0, 0))
+	if (!BN_rand(btmp, SERIAL_RAND_BITS, 0, 0))
 		goto error;
 	if (ai && !BN_to_ASN1_INTEGER(btmp, ai))
 		goto error;
diff --git a/tools/cert_create/src/key.c b/tools/cert_create/src/key.c
index 6435975..2857a3b 100644
--- a/tools/cert_create/src/key.c
+++ b/tools/cert_create/src/key.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -40,69 +40,25 @@
 
 static int key_create_rsa(key_t *key, int key_bits)
 {
-	BIGNUM *e;
-	RSA *rsa = NULL;
-
-	e = BN_new();
-	if (e == NULL) {
-		printf("Cannot create RSA exponent\n");
-		goto err;
-	}
-
-	if (!BN_set_word(e, RSA_F4)) {
-		printf("Cannot assign RSA exponent\n");
-		goto err;
-	}
-
-	rsa = RSA_new();
+	EVP_PKEY *rsa = EVP_RSA_gen(key_bits);
 	if (rsa == NULL) {
-		printf("Cannot create RSA key\n");
-		goto err;
-	}
-
-	if (!RSA_generate_key_ex(rsa, key_bits, e, NULL)) {
 		printf("Cannot generate RSA key\n");
-		goto err;
-	}
-
-	if (!EVP_PKEY_assign_RSA(key->key, rsa)) {
-		printf("Cannot assign RSA key\n");
-		goto err;
+		return 0;
 	}
-
-	BN_free(e);
+	key->key = rsa;
 	return 1;
-err:
-	RSA_free(rsa);
-	BN_free(e);
-	return 0;
 }
 
 #ifndef OPENSSL_NO_EC
 static int key_create_ecdsa(key_t *key, int key_bits)
 {
-	EC_KEY *ec;
-
-	ec = EC_KEY_new_by_curve_name(NID_X9_62_prime256v1);
+	EVP_PKEY *ec = EVP_EC_gen("prime256v1");
 	if (ec == NULL) {
-		printf("Cannot create EC key\n");
-		goto err;
-	}
-	if (!EC_KEY_generate_key(ec)) {
 		printf("Cannot generate EC key\n");
-		goto err;
-	}
-	EC_KEY_set_flags(ec, EC_PKEY_NO_PARAMETERS);
-	EC_KEY_set_asn1_flag(ec, OPENSSL_EC_NAMED_CURVE);
-	if (!EVP_PKEY_assign_EC_KEY(key->key, ec)) {
-		printf("Cannot assign EC key\n");
-		goto err;
+		return 0;
 	}
-
+	key->key = ec;
 	return 1;
-err:
-	EC_KEY_free(ec);
-	return 0;
 }
 #endif /* OPENSSL_NO_EC */
 
diff --git a/tools/cert_create/src/sha.c b/tools/cert_create/src/sha.c
index 3d977fb..06ef360 100644
--- a/tools/cert_create/src/sha.c
+++ b/tools/cert_create/src/sha.c
@@ -1,26 +1,38 @@
 /*
- * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2022, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
-#include <openssl/sha.h>
 #include <stdio.h>
 #include "debug.h"
 #include "key.h"
+#include <openssl/evp.h>
+#include <openssl/obj_mac.h>
 
 #define BUFFER_SIZE	256
 
+static int get_algorithm_nid(int hash_alg)
+{
+	int nids[] = {NID_sha256, NID_sha384, NID_sha512};
+	if (hash_alg < 0 || hash_alg >= sizeof(nids) / sizeof(*nids)) {
+		return NID_undef;
+	}
+	return nids[hash_alg];
+}
+
 int sha_file(int md_alg, const char *filename, unsigned char *md)
 {
 	FILE *inFile;
-	SHA256_CTX shaContext;
-	SHA512_CTX sha512Context;
+	EVP_MD_CTX *mdctx;
+	const EVP_MD *md_type;
 	int bytes;
+	int alg_nid;
+	unsigned int total_bytes;
 	unsigned char data[BUFFER_SIZE];
 
 	if ((filename == NULL) || (md == NULL)) {
-		ERROR("%s(): NULL argument\n", __FUNCTION__);
+		ERROR("%s(): NULL argument\n", __func__);
 		return 0;
 	}
 
@@ -30,26 +42,37 @@
 		return 0;
 	}
 
+	mdctx = EVP_MD_CTX_new();
+	if (mdctx == NULL) {
+		fclose(inFile);
+		ERROR("%s(): Could not create EVP MD context\n", __func__);
+		return 0;
+	}
+
-	if (md_alg == HASH_ALG_SHA384) {
-		SHA384_Init(&sha512Context);
-		while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
-			SHA384_Update(&sha512Context, data, bytes);
-		}
-		SHA384_Final(md, &sha512Context);
-	} else if (md_alg == HASH_ALG_SHA512) {
-		SHA512_Init(&sha512Context);
-		while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
-			SHA512_Update(&sha512Context, data, bytes);
-		}
-		SHA512_Final(md, &sha512Context);
-	} else {
-		SHA256_Init(&shaContext);
-		while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
-			SHA256_Update(&shaContext, data, bytes);
-		}
-		SHA256_Final(md, &shaContext);
+	alg_nid = get_algorithm_nid(md_alg);
+	if (alg_nid == NID_undef) {
+		ERROR("%s(): Invalid hash algorithm\n", __func__);
+		goto err;
 	}
 
+	md_type = EVP_get_digestbynid(alg_nid);
+	if (EVP_DigestInit_ex(mdctx, md_type, NULL) == 0) {
+		ERROR("%s(): Could not initialize EVP MD digest\n", __func__);
+		goto err;
+	}
+
+	while ((bytes = fread(data, 1, BUFFER_SIZE, inFile)) != 0) {
+		EVP_DigestUpdate(mdctx, data, bytes);
+	}
+	EVP_DigestFinal_ex(mdctx, md, &total_bytes);
+
 	fclose(inFile);
+	EVP_MD_CTX_free(mdctx);
 	return 1;
+
+err:
+	fclose(inFile);
+	EVP_MD_CTX_free(mdctx);
+	return 0;
 }
+
diff --git a/tools/encrypt_fw/Makefile b/tools/encrypt_fw/Makefile
index 96dff23..60bd8ea 100644
--- a/tools/encrypt_fw/Makefile
+++ b/tools/encrypt_fw/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2019-2020, Linaro Limited. All rights reserved.
+# Copyright (c) 2019-2022, Linaro Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -39,7 +39,14 @@
 # Make soft links and include from local directory otherwise wrong headers
 # could get pulled in from firmware tree.
 INC_DIR := -I ./include -I ../../include/tools_share -I ${OPENSSL_DIR}/include
-LIB_DIR := -L ${OPENSSL_DIR}/lib
+
+# Include library directories where OpenSSL library files are located.
+# For a normal installation (i.e.: when ${OPENSSL_DIR} = /usr or
+# /usr/local), binaries are located under the ${OPENSSL_DIR}/lib/
+# directory. However, for a local build of OpenSSL, the built binaries are
+# located under the main project directory (i.e.: ${OPENSSL_DIR}, not
+# ${OPENSSL_DIR}/lib/).
+LIB_DIR := -L ${OPENSSL_DIR}/lib -L ${OPENSSL_DIR}
 LIB := -lssl -lcrypto
 
 HOSTCC ?= gcc
diff --git a/tools/fiptool/Makefile b/tools/fiptool/Makefile
index 7c2a083..e6aeba9 100644
--- a/tools/fiptool/Makefile
+++ b/tools/fiptool/Makefile
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2014-2021, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2014-2022, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -22,7 +22,14 @@
 else
   HOSTCCFLAGS += -O2
 endif
-LDLIBS := -L${OPENSSL_DIR}/lib -lcrypto
+
+# Include library directories where OpenSSL library files are located.
+# For a normal installation (i.e.: when ${OPENSSL_DIR} = /usr or
+# /usr/local), binaries are located under the ${OPENSSL_DIR}/lib/
+# directory. However, for a local build of OpenSSL, the built binaries are
+# located under the main project directory (i.e.: ${OPENSSL_DIR}, not
+# ${OPENSSL_DIR}/lib/).
+LDLIBS := -L${OPENSSL_DIR}/lib -L${OPENSSL_DIR} -lcrypto
 
 ifeq (${V},0)
   Q := @