diff --git a/common/uuid.c b/common/uuid.c
index dd3c7b0..ac6db50 100644
--- a/common/uuid.c
+++ b/common/uuid.c
@@ -73,6 +73,7 @@
 int read_uuid(uint8_t *dest, char *uuid)
 {
 	int err;
+	uint8_t *dest_start = dest;
 
 	/* Check that we have enough characters */
 	if (strnlen(uuid, UUID_STRING_LENGTH) != UUID_STRING_LENGTH) {
@@ -124,7 +125,7 @@
 	if (err < 0) {
 		WARN("Error parsing UUID\n");
 		/* Clear the buffer on error */
-		memset((void *)dest, '\0', UUID_BYTES_LENGTH * sizeof(uint8_t));
+		memset((void *)dest_start, '\0', UUID_BYTES_LENGTH * sizeof(uint8_t));
 		return -EINVAL;
 	}
 
diff --git a/docs/about/maintainers.rst b/docs/about/maintainers.rst
index 97e1fa8..725e96b 100644
--- a/docs/about/maintainers.rst
+++ b/docs/about/maintainers.rst
@@ -399,6 +399,7 @@
 :|G|: `vishnu-banavath`_
 :|F|: plat/arm/board/corstone700
 :|F|: plat/arm/board/a5ds
+:|F|: plat/arm/board/diphda
 
 Arm Reference Design platform ports
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
@@ -493,8 +494,8 @@
 
 NXP i.MX 8 platform port
 ^^^^^^^^^^^^^^^^^^^^^^^^
-:|M|: Anson Huang <Anson.Huang@nxp.com>
-:|G|: `Anson-Huang`_
+:|M|: Peng Fan <peng.fan@nxp.com>
+:|G|: `MrVan`_
 :|F|: docs/plat/imx8.rst
 :|F|: plat/imx/
 
diff --git a/docs/plat/arm/diphda/index.rst b/docs/plat/arm/diphda/index.rst
new file mode 100644
index 0000000..27afda4
--- /dev/null
+++ b/docs/plat/arm/diphda/index.rst
@@ -0,0 +1,61 @@
+Diphda Platform
+==========================
+
+Some of the features of the Diphda platform referenced in TF-A include:
+
+- Cortex-A35 application processor (64-bit mode)
+- Secure Enclave
+- GIC-400
+- Trusted Board Boot
+
+Boot Sequence
+-------------
+
+The board boot relies on CoT (chain of trust). The trusted-firmware-a
+BL2 is extracted from the FIP and verified by the Secure Enclave
+processor. BL2 verification relies on the signature area at the
+beginning of the BL2 image. This area is needed by the SecureEnclave
+bootloader.
+
+Then, the application processor is released from reset and starts by
+executing BL2.
+
+BL2 performs the actions described in the trusted-firmware-a TBB design
+document.
+
+Build Procedure (TF-A only)
+~~~~~~~~~~~~~~~~~~~~~~~~~~~
+
+-  Obtain AArch64 ELF bare-metal target `toolchain <https://developer.arm.com/tools-and-software/open-source-software/developer-tools/gnu-toolchain/gnu-a/downloads>`_.
+   Set the CROSS_COMPILE environment variable to point to the toolchain folder.
+
+-  Build TF-A:
+
+   .. code:: shell
+
+      make LD=aarch64-none-elf-ld \
+      CC=aarch64-none-elf-gcc \
+      V=1 \
+      BUILD_BASE=<path to the build folder> \
+      PLAT=diphda \
+      SPD=spmd \
+      SPMD_SPM_AT_SEL2=0 \
+      DEBUG=1 \
+      MBEDTLS_DIR=mbedtls \
+      OPENSSL_DIR=<path to openssl usr folder> \
+      RUNTIME_SYSROOT=<path to the sysroot> \
+      ARCH=aarch64 \
+      TARGET_PLATFORM=<fpga or fvp> \
+      ENABLE_PIE=1 \
+      BL2_AT_EL3=1 \
+      CREATE_KEYS=1 \
+      GENERATE_COT=1 \
+      TRUSTED_BOARD_BOOT=1 \
+      COT=tbbr \
+      ARM_ROTPK_LOCATION=devel_rsa \
+      ROT_KEY=plat/arm/board/common/rotpk/arm_rotprivk_rsa.pem \
+      BL32=<path to optee binary> \
+      BL33=<path to u-boot binary> \
+      bl2
+
+*Copyright (c) 2021, Arm Limited. All rights reserved.*
diff --git a/docs/plat/arm/index.rst b/docs/plat/arm/index.rst
index f72992b..f50dab5 100644
--- a/docs/plat/arm/index.rst
+++ b/docs/plat/arm/index.rst
@@ -12,6 +12,7 @@
    arm_fpga/index
    arm-build-options
    morello/index
+   diphda/index
 
 This chapter holds documentation related to Arm's development platforms,
 including both software models (FVPs) and hardware development boards
@@ -19,4 +20,4 @@
 
 --------------
 
-*Copyright (c) 2019, Arm Limited. All rights reserved.*
+*Copyright (c) 2021, Arm Limited. All rights reserved.*
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
index 668416c..b1139b5 100644
--- a/drivers/arm/gic/v3/gicv3_main.c
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -332,6 +332,8 @@
 	write_icc_igrpen1_el3(read_icc_igrpen1_el3() |
 				IGRPEN1_EL3_ENABLE_G1S_BIT);
 	isb();
+	/* Add DSB to ensure visibility of System register writes */
+	dsb();
 }
 
 /*******************************************************************************
@@ -363,6 +365,8 @@
 
 	/* Synchronise accesses to group enable registers */
 	isb();
+	/* Add DSB to ensure visibility of System register writes */
+	dsb();
 
 	/* Mark the connected core as asleep */
 	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
diff --git a/drivers/scmi-msg/common.h b/drivers/scmi-msg/common.h
index ef5953b..62f3087 100644
--- a/drivers/scmi-msg/common.h
+++ b/drivers/scmi-msg/common.h
@@ -13,6 +13,7 @@
 
 #include "base.h"
 #include "clock.h"
+#include "power_domain.h"
 #include "reset_domain.h"
 
 #define SCMI_VERSION			0x20000U
@@ -111,6 +112,13 @@
 scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg);
 
 /*
+ * scmi_msg_get_pd_handler - Return a handler for a power domain message
+ * @msg - message to process
+ * Return a function handler for the message or NULL
+ */
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg);
+
+/*
  * Process Read, process and write response for input SCMI message
  *
  * @msg: SCMI message context
diff --git a/drivers/scmi-msg/entry.c b/drivers/scmi-msg/entry.c
index ea3efa2..3537fbe 100644
--- a/drivers/scmi-msg/entry.c
+++ b/drivers/scmi-msg/entry.c
@@ -11,6 +11,31 @@
 
 #include "common.h"
 
+#pragma weak scmi_msg_get_clock_handler
+#pragma weak scmi_msg_get_rstd_handler
+#pragma weak scmi_msg_get_pd_handler
+#pragma weak scmi_msg_get_voltage_handler
+
+scmi_msg_handler_t scmi_msg_get_clock_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_rstd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
+scmi_msg_handler_t scmi_msg_get_voltage_handler(struct scmi_msg *msg __unused)
+{
+	return NULL;
+}
+
 void scmi_status_response(struct scmi_msg *msg, int32_t status)
 {
 	assert(msg->out && msg->out_size >= sizeof(int32_t));
@@ -47,6 +72,9 @@
 	case SCMI_PROTOCOL_ID_RESET_DOMAIN:
 		handler = scmi_msg_get_rstd_handler(msg);
 		break;
+	case SCMI_PROTOCOL_ID_POWER_DOMAIN:
+		handler = scmi_msg_get_pd_handler(msg);
+		break;
 	default:
 		break;
 	}
diff --git a/drivers/scmi-msg/power_domain.c b/drivers/scmi-msg/power_domain.c
new file mode 100644
index 0000000..c4e1289
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.c
@@ -0,0 +1,239 @@
+// SPDX-License-Identifier: BSD-3-Clause
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2019-2020, Linaro Limited
+ */
+#include <cdefs.h>
+#include <string.h>
+
+#include <drivers/scmi-msg.h>
+#include <drivers/scmi.h>
+#include <lib/utils_def.h>
+
+#include "common.h"
+
+#pragma weak plat_scmi_pd_count
+#pragma weak plat_scmi_pd_get_name
+#pragma weak plat_scmi_pd_get_state
+#pragma weak plat_scmi_pd_set_state
+#pragma weak plat_scmi_pd_statistics
+#pragma weak plat_scmi_pd_get_attributes
+
+static bool message_id_is_supported(size_t message_id);
+
+size_t plat_scmi_pd_count(unsigned int agent_id __unused)
+{
+	return 0U;
+}
+
+const char *plat_scmi_pd_get_name(unsigned int agent_id __unused,
+				  unsigned int pd_id __unused)
+{
+	return NULL;
+}
+
+unsigned int plat_scmi_pd_statistics(unsigned int agent_id __unused,
+				     unsigned long *pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_attributes(unsigned int agent_id __unused,
+					 unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+unsigned int plat_scmi_pd_get_state(unsigned int agent_id __unused,
+				    unsigned int pd_id __unused)
+{
+	return 0U;
+}
+
+int32_t plat_scmi_pd_set_state(unsigned int agent_id __unused,
+			       unsigned int flags __unused,
+			       unsigned int pd_id __unused,
+			       unsigned int state __unused)
+{
+	return SCMI_NOT_SUPPORTED;
+}
+
+static void report_version(struct scmi_msg *msg)
+{
+	struct scmi_protocol_version_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		.version = SCMI_PROTOCOL_VERSION_PD,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_attributes(struct scmi_msg *msg)
+{
+	unsigned long addr = 0UL;
+	unsigned int len;
+
+	struct scmi_protocol_attributes_p2a_pd return_values = {
+		.status = SCMI_SUCCESS,
+	};
+
+	if (msg->in_size != 0) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	return_values.attributes = plat_scmi_pd_count(msg->agent_id);
+	len = plat_scmi_pd_statistics(msg->agent_id, &addr);
+	if (len != 0U) {
+		return_values.statistics_addr_low = (unsigned int)addr;
+		return_values.statistics_addr_high = (uint32_t)(addr >> 32);
+		return_values.statistics_len = len;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void report_message_attributes(struct scmi_msg *msg)
+{
+	struct scmi_protocol_message_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_protocol_message_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+		/* For this protocol, attributes shall be zero */
+		.attributes = 0U,
+	};
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	if (!message_id_is_supported(in_args->message_id)) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_attributes(struct scmi_msg *msg)
+{
+	const struct scmi_pd_attributes_a2p *in_args = (void *)msg->in;
+	struct scmi_pd_attributes_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	const char *name = NULL;
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	name = plat_scmi_pd_get_name(msg->agent_id, pd_id);
+	if (name == NULL) {
+		scmi_status_response(msg, SCMI_NOT_FOUND);
+		return;
+	}
+
+	COPY_NAME_IDENTIFIER(return_values.pd_name, name);
+
+	return_values.attributes = plat_scmi_pd_get_attributes(msg->agent_id, pd_id);
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_get(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_get_a2p *in_args = (void *)msg->in;
+	unsigned int state = 0U;
+	struct scmi_pd_state_get_p2a return_values = {
+		.status = SCMI_SUCCESS,
+	};
+	unsigned int pd_id = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	state = plat_scmi_pd_get_state(msg->agent_id, pd_id);
+
+	return_values.power_state = state;
+
+	scmi_write_response(msg, &return_values, sizeof(return_values));
+}
+
+static void scmi_pd_state_set(struct scmi_msg *msg)
+{
+	const struct scmi_pd_state_set_a2p *in_args = (void *)msg->in;
+	unsigned int flags = 0U;
+	int32_t status = 0;
+	unsigned int pd_id = 0U;
+	unsigned int state = 0U;
+
+	if (msg->in_size != sizeof(*in_args)) {
+		scmi_status_response(msg, SCMI_PROTOCOL_ERROR);
+		return;
+	}
+
+	pd_id = SPECULATION_SAFE_VALUE(in_args->pd_id);
+
+	if (pd_id >= plat_scmi_pd_count(msg->agent_id)) {
+		scmi_status_response(msg, SCMI_INVALID_PARAMETERS);
+		return;
+	}
+
+	flags = SPECULATION_SAFE_VALUE(in_args->flags);
+	state = SPECULATION_SAFE_VALUE(in_args->power_state);
+
+	status = plat_scmi_pd_set_state(msg->agent_id, flags, pd_id, state);
+
+	scmi_status_response(msg, status);
+}
+
+static const scmi_msg_handler_t scmi_pd_handler_table[] = {
+	[SCMI_PROTOCOL_VERSION] = report_version,
+	[SCMI_PROTOCOL_ATTRIBUTES] = report_attributes,
+	[SCMI_PROTOCOL_MESSAGE_ATTRIBUTES] = report_message_attributes,
+	[SCMI_PD_ATTRIBUTES] = scmi_pd_attributes,
+	[SCMI_PD_STATE_SET] = scmi_pd_state_set,
+	[SCMI_PD_STATE_GET] = scmi_pd_state_get,
+};
+
+static bool message_id_is_supported(size_t message_id)
+{
+	return (message_id < ARRAY_SIZE(scmi_pd_handler_table)) &&
+	       (scmi_pd_handler_table[message_id] != NULL);
+}
+
+scmi_msg_handler_t scmi_msg_get_pd_handler(struct scmi_msg *msg)
+{
+	const size_t array_size = ARRAY_SIZE(scmi_pd_handler_table);
+	unsigned int message_id = SPECULATION_SAFE_VALUE(msg->message_id);
+
+	if (message_id >= array_size) {
+		VERBOSE("pd handle not found %u", msg->message_id);
+		return NULL;
+	}
+
+	return scmi_pd_handler_table[message_id];
+}
diff --git a/drivers/scmi-msg/power_domain.h b/drivers/scmi-msg/power_domain.h
new file mode 100644
index 0000000..48551fd
--- /dev/null
+++ b/drivers/scmi-msg/power_domain.h
@@ -0,0 +1,72 @@
+/* SPDX-License-Identifier: BSD-3-Clause */
+/*
+ * Copyright 2021 NXP
+ */
+
+#ifndef SCMI_MSG_PD_H
+#define SCMI_MSG_PD_H
+
+#include <stdint.h>
+
+#include <lib/utils_def.h>
+
+#define SCMI_PROTOCOL_VERSION_PD	0x21000U
+
+/*
+ * Identifiers of the SCMI POWER DOMAIN Protocol commands
+ */
+enum scmi_pd_command_id {
+	SCMI_PD_ATTRIBUTES = 0x003,
+	SCMI_PD_STATE_SET = 0x004,
+	SCMI_PD_STATE_GET = 0x005,
+};
+
+/* Protocol attributes */
+struct scmi_pd_attributes_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_protocol_attributes_p2a_pd {
+	int32_t status;
+	uint32_t attributes;
+	uint32_t statistics_addr_low;
+	uint32_t statistics_addr_high;
+	uint32_t statistics_len;
+};
+
+#define SCMI_PD_NAME_LENGTH_MAX	16U
+
+struct scmi_pd_attributes_p2a {
+	int32_t status;
+	uint32_t attributes;
+	char pd_name[SCMI_PD_NAME_LENGTH_MAX];
+};
+
+/*
+ * Power Domain State Get
+ */
+
+struct scmi_pd_state_get_a2p {
+	uint32_t pd_id;
+};
+
+struct scmi_pd_state_get_p2a {
+	int32_t status;
+	uint32_t power_state;
+};
+
+/*
+ * Power domain State Set
+ */
+
+struct scmi_pd_state_set_a2p {
+	uint32_t flags;
+	uint32_t pd_id;
+	uint32_t power_state;
+};
+
+struct scmi_pd_state_set_p2a {
+	int32_t status;
+};
+
+#endif /* SCMI_MSG_PD_H */
diff --git a/drivers/scmi-msg/smt.c b/drivers/scmi-msg/smt.c
index b08ee06..9b079c7 100644
--- a/drivers/scmi-msg/smt.c
+++ b/drivers/scmi-msg/smt.c
@@ -44,12 +44,12 @@
 	assert_scmi_message_max_length_fits_in_smt_buffer_slot);
 
 /* Flag set in smt_header::status when SMT does not contain pending message */
-#define SMT_STATUS_FREE			BIT(0)
+#define SMT_STATUS_FREE			BIT_32(0)
 /* Flag set in smt_header::status when SMT reports an error */
-#define SMT_STATUS_ERROR		BIT(1)
+#define SMT_STATUS_ERROR		BIT_32(1)
 
 /* Flag set in smt_header::flags when SMT uses interrupts */
-#define SMT_FLAG_INTR_ENABLED		BIT(1)
+#define SMT_FLAG_INTR_ENABLED		BIT_32(1)
 
 /* Bit fields packed in smt_header::message_header */
 #define SMT_MSG_ID_MASK			GENMASK_32(7, 0)
@@ -133,7 +133,7 @@
 			  sizeof(smt_hdr->message_header);
 
 	if (in_payload_size > SCMI_PLAYLOAD_MAX) {
-		VERBOSE("SCMI payload too big %u", in_payload_size);
+		VERBOSE("SCMI payload too big %zu", in_payload_size);
 		goto out;
 	}
 
diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h
index 3383a3b..c12dbc4 100644
--- a/include/arch/aarch64/arch.h
+++ b/include/arch/aarch64/arch.h
@@ -263,6 +263,9 @@
 #define ID_AA64MMFR1_EL1_PAN2_SUPPORTED		ULL(0x2)
 #define ID_AA64MMFR1_EL1_PAN3_SUPPORTED		ULL(0x3)
 
+#define ID_AA64MMFR1_EL1_VHE_SHIFT		U(8)
+#define ID_AA64MMFR1_EL1_VHE_MASK		ULL(0xf)
+
 /* ID_AA64MMFR2_EL1 definitions */
 #define ID_AA64MMFR2_EL1		S3_0_C0_C7_2
 
@@ -390,7 +393,8 @@
 
 #define SCTLR_ATA0_BIT		(ULL(1) << 42)
 #define SCTLR_ATA_BIT		(ULL(1) << 43)
-#define SCTLR_DSSBS_BIT		(ULL(1) << 44)
+#define SCTLR_DSSBS_SHIFT	U(44)
+#define SCTLR_DSSBS_BIT		(ULL(1) << SCTLR_DSSBS_SHIFT)
 #define SCTLR_TWEDEn_BIT	(ULL(1) << 45)
 #define SCTLR_TWEDEL_SHIFT	U(46)
 #define SCTLR_TWEDEL_MASK	ULL(0xf)
@@ -570,8 +574,16 @@
 #define SPSR_EL_SHIFT		U(2)
 #define SPSR_EL_WIDTH		U(2)
 
+#define SPSR_SSBS_SHIFT_AARCH64 U(12)
+#define SPSR_SSBS_BIT_AARCH64	(ULL(1) << SPSR_SSBS_SHIFT_AARCH64)
+#define SPSR_SSBS_SHIFT_AARCH32 U(23)
+#define SPSR_SSBS_BIT_AARCH32	(ULL(1) << SPSR_SSBS_SHIFT_AARCH32)
+
+#define SPSR_PAN_BIT		BIT_64(22)
+
+#define SPSR_DIT_BIT		BIT(24)
+
-#define SPSR_SSBS_BIT_AARCH64	BIT_64(12)
-#define SPSR_SSBS_BIT_AARCH32	BIT_64(23)
+#define SPSR_TCO_BIT_AARCH64	BIT_64(25)
 
 #define DISABLE_ALL_EXCEPTIONS \
 		(DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT | DAIF_DBG_BIT)
diff --git a/include/arch/aarch64/arch_features.h b/include/arch/aarch64/arch_features.h
index 47a797a..dc0b7f3 100644
--- a/include/arch/aarch64/arch_features.h
+++ b/include/arch/aarch64/arch_features.h
@@ -17,6 +17,18 @@
 	return true;
 }
 
+static inline bool is_armv8_1_pan_present(void)
+{
+	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_PAN_SHIFT) &
+		ID_AA64MMFR1_EL1_PAN_MASK) != 0U;
+}
+
+static inline bool is_armv8_1_vhe_present(void)
+{
+	return ((read_id_aa64mmfr1_el1() >> ID_AA64MMFR1_EL1_VHE_SHIFT) &
+		ID_AA64MMFR1_EL1_VHE_MASK) != 0U;
+}
+
 static inline bool is_armv8_2_ttcnp_present(void)
 {
 	return ((read_id_aa64mmfr2_el1() >> ID_AA64MMFR2_EL1_CNP_SHIFT) &
diff --git a/include/arch/aarch64/asm_macros.S b/include/arch/aarch64/asm_macros.S
index 464c05b..7706cd8 100644
--- a/include/arch/aarch64/asm_macros.S
+++ b/include/arch/aarch64/asm_macros.S
@@ -10,10 +10,6 @@
 #include <common/asm_macros_common.S>
 #include <lib/spinlock.h>
 
-#if ENABLE_BTI && !ARM_ARCH_AT_LEAST(8, 5)
-#error Branch Target Identification requires ARM_ARCH_MINOR >= 5
-#endif
-
 /*
  * TLBI instruction with type specifier that implements the workaround for
  * errata 813419 of Cortex-A57 or errata 1286807 of Cortex-A76.
diff --git a/include/common/bl_common.h b/include/common/bl_common.h
index 77fb1f6..e33840c 100644
--- a/include/common/bl_common.h
+++ b/include/common/bl_common.h
@@ -106,6 +106,10 @@
 IMPORT_SYM(uintptr_t, __RO_START__,		BL_CODE_BASE);
 IMPORT_SYM(uintptr_t, __RO_END__,		BL_CODE_END);
 #endif
+#if SEPARATE_NOBITS_REGION
+IMPORT_SYM(uintptr_t, __NOBITS_START__,		BL_NOBITS_BASE);
+IMPORT_SYM(uintptr_t, __NOBITS_END__,		BL_NOBITS_END);
+#endif
 IMPORT_SYM(uintptr_t, __RW_END__,		BL_END);
 
 #if defined(IMAGE_BL1)
diff --git a/include/lib/extensions/amu.h b/include/lib/extensions/amu.h
index 3a70e4f..3a254c9 100644
--- a/include/lib/extensions/amu.h
+++ b/include/lib/extensions/amu.h
@@ -13,6 +13,7 @@
 #include <lib/cassert.h>
 #include <lib/utils_def.h>
 
+#include <context.h>
 #include <platform_def.h>
 
 /* All group 0 counters */
@@ -80,7 +81,11 @@
 };
 
 unsigned int amu_get_version(void);
+#if __aarch64__
+void amu_enable(bool el2_unused, cpu_context_t *ctx);
+#else
 void amu_enable(bool el2_unused);
+#endif
 
 /* Group 0 configuration helpers */
 uint64_t amu_group0_cnt_read(unsigned int idx);
diff --git a/lib/el3_runtime/aarch64/context.S b/lib/el3_runtime/aarch64/context.S
index d610fd4..40e7ddf 100644
--- a/lib/el3_runtime/aarch64/context.S
+++ b/lib/el3_runtime/aarch64/context.S
@@ -903,16 +903,11 @@
 
 #if IMAGE_BL31
 	/* ----------------------------------------------------------
-	 * Restore CPTR_EL3, ZCR_EL3 for SVE support.
-	 * If SVE is not supported - skip the restoration.
+	 * Restore CPTR_EL3.
 	 * ZCR is only restored if SVE is supported and enabled.
 	 * Synchronization is required before zcr_el3 is addressed.
 	 * ----------------------------------------------------------
 	 */
-	mrs	x17, id_aa64pfr0_el1
-	ubfx	x17, x17, ID_AA64PFR0_SVE_SHIFT, ID_AA64PFR0_SVE_LENGTH
-	cbz	x17, sve_not_enabled
-
 	ldp	x19, x20, [sp, #CTX_EL3STATE_OFFSET + CTX_CPTR_EL3]
 	msr	cptr_el3, x19
 
diff --git a/lib/el3_runtime/aarch64/context_mgmt.c b/lib/el3_runtime/aarch64/context_mgmt.c
index 7a25151..7c6f953 100644
--- a/lib/el3_runtime/aarch64/context_mgmt.c
+++ b/lib/el3_runtime/aarch64/context_mgmt.c
@@ -25,6 +25,7 @@
 #include <lib/extensions/twed.h>
 #include <lib/utils.h>
 
+static void enable_extensions_secure(cpu_context_t *ctx);
 
 /*******************************************************************************
  * Context management library initialisation routine. This library is used by
@@ -178,19 +179,13 @@
 	 *  indicated by the interrupt routing model for BL31.
 	 */
 	scr_el3 |= get_scr_el3_from_routing_model(security_state);
-
-#if ENABLE_SVE_FOR_NS
-	if (security_state == NON_SECURE) {
-		sve_enable(ctx);
-	}
 #endif
-#if ENABLE_SVE_FOR_SWD
+
+	/* Save the initialized value of CPTR_EL3 register */
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, read_cptr_el3());
 	if (security_state == SECURE) {
-		sve_enable(ctx);
+		enable_extensions_secure(ctx);
 	}
-#endif
-
-#endif
 
 	/*
 	 * SCR_EL3.HCE: Enable HVC instructions if next execution state is
@@ -335,7 +330,7 @@
  * When EL2 is implemented but unused `el2_unused` is non-zero, otherwise
  * it is zero.
  ******************************************************************************/
-static void enable_extensions_nonsecure(bool el2_unused)
+static void enable_extensions_nonsecure(bool el2_unused, cpu_context_t *ctx)
 {
 #if IMAGE_BL31
 #if ENABLE_SPE_FOR_LOWER_ELS
@@ -343,9 +338,13 @@
 #endif
 
 #if ENABLE_AMU
-	amu_enable(el2_unused);
+	amu_enable(el2_unused, ctx);
 #endif
 
+#if ENABLE_SVE_FOR_NS
+	sve_enable(ctx);
+#endif
+
 #if ENABLE_MPAM_FOR_LOWER_ELS
 	mpam_enable(el2_unused);
 #endif
@@ -353,6 +352,18 @@
 }
 
 /*******************************************************************************
+ * Enable architecture extensions on first entry to Secure world.
+ ******************************************************************************/
+static void enable_extensions_secure(cpu_context_t *ctx)
+{
+#if IMAGE_BL31
+#if ENABLE_SVE_FOR_SWD
+	sve_enable(ctx);
+#endif
+#endif
+}
+
+/*******************************************************************************
  * The following function initializes the cpu_context for a CPU specified by
  * its `cpu_idx` for first use, and sets the initial entrypoint state as
  * specified by the entry_point_info structure.
@@ -586,7 +597,7 @@
 			write_cnthp_ctl_el2(CNTHP_CTL_RESET_VAL &
 						~(CNTHP_CTL_ENABLE_BIT));
 		}
-		enable_extensions_nonsecure(el2_unused);
+		enable_extensions_nonsecure(el2_unused, ctx);
 	}
 
 	cm_el1_sysregs_context_restore(security_state);
diff --git a/lib/extensions/amu/aarch64/amu.c b/lib/extensions/amu/aarch64/amu.c
index 24c3737..295c0d5 100644
--- a/lib/extensions/amu/aarch64/amu.c
+++ b/lib/extensions/amu/aarch64/amu.c
@@ -46,7 +46,7 @@
  * Enable counters. This function is meant to be invoked
  * by the context management library before exiting from EL3.
  */
-void amu_enable(bool el2_unused)
+void amu_enable(bool el2_unused, cpu_context_t *ctx)
 {
 	uint64_t v;
 	unsigned int amu_version = amu_get_version();
@@ -88,12 +88,13 @@
 	}
 
 	/*
-	 * CPTR_EL3.TAM: Set to zero so that any accesses to
+	 * Retrieve and update the CPTR_EL3 value from the context mentioned
+	 * in 'ctx'. Set CPTR_EL3.TAM to zero so that any accesses to
 	 * the Activity Monitor registers do not trap to EL3.
 	 */
-	v = read_cptr_el3();
+	v = read_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3);
 	v &= ~TAM_BIT;
-	write_cptr_el3(v);
+	write_ctx_reg(get_el3state_ctx(ctx), CTX_CPTR_EL3, v);
 
 	/* Enable group 0 counters */
 	write_amcntenset0_el0(AMU_GROUP0_COUNTERS_MASK);
diff --git a/lib/extensions/sve/sve.c b/lib/extensions/sve/sve.c
index 7043cc2..2702c30 100644
--- a/lib/extensions/sve/sve.c
+++ b/lib/extensions/sve/sve.c
@@ -27,11 +27,13 @@
 
 void sve_enable(cpu_context_t *context)
 {
+	u_register_t cptr_el3;
+
 	if (!sve_supported()) {
 		return;
 	}
 
-	u_register_t cptr_el3 = read_cptr_el3();
+	cptr_el3 = read_ctx_reg(get_el3state_ctx(context), CTX_CPTR_EL3);
 
 	/* Enable access to SVE functionality for all ELs. */
 	cptr_el3 = (cptr_el3 | CPTR_EZ_BIT) & ~(TFP_BIT);
diff --git a/plat/allwinner/common/include/platform_def.h b/plat/allwinner/common/include/platform_def.h
index 4893368..56a2ad6 100644
--- a/plat/allwinner/common/include/platform_def.h
+++ b/plat/allwinner/common/include/platform_def.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -13,9 +13,6 @@
 
 #include <sunxi_mmap.h>
 
-/* The SCP firmware is allocated the last 16KiB of SRAM A2. */
-#define SUNXI_SCP_SIZE			0x4000
-
 #ifdef SUNXI_BL31_IN_DRAM
 
 #define BL31_BASE			SUNXI_DRAM_BASE
@@ -31,7 +28,6 @@
 #define BL31_BASE			(SUNXI_SRAM_A2_BASE + 0x4000)
 #define BL31_LIMIT			(SUNXI_SRAM_A2_BASE + \
 					 SUNXI_SRAM_A2_SIZE - SUNXI_SCP_SIZE)
-#define SUNXI_SCP_BASE			BL31_LIMIT
 
 /* Overwrite U-Boot SPL, but reserve the first page for the SPL header. */
 #define BL31_NOBITS_BASE		(SUNXI_SRAM_A1_BASE + 0x1000)
@@ -39,12 +35,14 @@
 
 #define MAX_XLAT_TABLES			1
 #define PLAT_VIRT_ADDR_SPACE_SIZE	(1ULL << 28)
-#define SUNXI_BL33_VIRT_BASE		(SUNXI_DRAM_VIRT_BASE + SUNXI_DRAM_SEC_SIZE)
 
-#endif /* SUNXI_BL31_IN_DRAM */
+#define SUNXI_BL33_VIRT_BASE		SUNXI_DRAM_VIRT_BASE
 
-/* How much memory to reserve as secure for BL32, if configured */
-#define SUNXI_DRAM_SEC_SIZE		(32U << 20)
+/* The SCP firmware is allocated the last 16KiB of SRAM A2. */
+#define SUNXI_SCP_BASE			BL31_LIMIT
+#define SUNXI_SCP_SIZE			0x4000
+
+#endif /* SUNXI_BL31_IN_DRAM */
 
 /* How much DRAM to map (to map BL33, for fetching the DTB from U-Boot) */
 #define SUNXI_DRAM_MAP_SIZE		(64U << 20)
@@ -52,7 +50,8 @@
 #define CACHE_WRITEBACK_SHIFT		6
 #define CACHE_WRITEBACK_GRANULE		(1 << CACHE_WRITEBACK_SHIFT)
 
-#define MAX_MMAP_REGIONS		(3 + PLATFORM_MMAP_REGIONS)
+#define MAX_STATIC_MMAP_REGIONS		3
+#define MAX_MMAP_REGIONS		(5 + MAX_STATIC_MMAP_REGIONS)
 
 #define PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
 	(SUNXI_SRAM_A2_BASE + SUNXI_SRAM_A2_SIZE - 0x200)
@@ -72,7 +71,6 @@
 #define PLATFORM_CORE_COUNT		(PLATFORM_CLUSTER_COUNT * \
 					 PLATFORM_MAX_CPUS_PER_CLUSTER)
 #define PLATFORM_MAX_CPUS_PER_CLUSTER	U(4)
-#define PLATFORM_MMAP_REGIONS		5
 #define PLATFORM_STACK_SIZE		(0x1000 / PLATFORM_CORE_COUNT)
 
 #ifndef SPD_none
diff --git a/plat/allwinner/common/sunxi_common.c b/plat/allwinner/common/sunxi_common.c
index d47d360..82410b1 100644
--- a/plat/allwinner/common/sunxi_common.c
+++ b/plat/allwinner/common/sunxi_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2020, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -14,17 +14,11 @@
 #include <sunxi_mmap.h>
 #include <sunxi_private.h>
 
-static const mmap_region_t sunxi_mmap[PLATFORM_MMAP_REGIONS + 1] = {
+static const mmap_region_t sunxi_mmap[MAX_STATIC_MMAP_REGIONS + 1] = {
 	MAP_REGION_FLAT(SUNXI_SRAM_BASE, SUNXI_SRAM_SIZE,
-			MT_RW_DATA | MT_SECURE),
-#ifdef SUNXI_SCP_BASE
-	MAP_REGION_FLAT(SUNXI_SCP_BASE, SUNXI_SCP_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
-#endif
 	MAP_REGION_FLAT(SUNXI_DEV_BASE, SUNXI_DEV_SIZE,
 			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER),
-	MAP_REGION(SUNXI_DRAM_BASE, SUNXI_DRAM_VIRT_BASE, SUNXI_DRAM_SEC_SIZE,
-		   MT_RW_DATA | MT_SECURE),
 	MAP_REGION(PRELOADED_BL33_BASE, SUNXI_BL33_VIRT_BASE,
 		   SUNXI_DRAM_MAP_SIZE, MT_RW_DATA | MT_NS),
 	{},
@@ -40,12 +34,24 @@
 	mmap_add_region(BL_CODE_BASE, BL_CODE_BASE,
 			BL_CODE_END - BL_CODE_BASE,
 			MT_CODE | MT_SECURE);
+	mmap_add_region(BL_CODE_END, BL_CODE_END,
+			BL_END - BL_CODE_END,
+			MT_RW_DATA | MT_SECURE);
+#if SEPARATE_CODE_AND_RODATA
 	mmap_add_region(BL_RO_DATA_BASE, BL_RO_DATA_BASE,
 			BL_RO_DATA_END - BL_RO_DATA_BASE,
 			MT_RO_DATA | MT_SECURE);
+#endif
+#if SEPARATE_NOBITS_REGION
+	mmap_add_region(BL_NOBITS_BASE, BL_NOBITS_BASE,
+			BL_NOBITS_END - BL_NOBITS_BASE,
+			MT_RW_DATA | MT_SECURE);
+#endif
+#if USE_COHERENT_MEM
 	mmap_add_region(BL_COHERENT_RAM_BASE, BL_COHERENT_RAM_BASE,
 			BL_COHERENT_RAM_END - BL_COHERENT_RAM_BASE,
 			MT_DEVICE | MT_RW | MT_SECURE | MT_EXECUTE_NEVER);
+#endif
 
 	mmap_add(sunxi_mmap);
 	init_xlat_tables();
diff --git a/plat/allwinner/common/sunxi_scpi_pm.c b/plat/allwinner/common/sunxi_scpi_pm.c
index 74763ef..eb37daa 100644
--- a/plat/allwinner/common/sunxi_scpi_pm.c
+++ b/plat/allwinner/common/sunxi_scpi_pm.c
@@ -212,7 +212,6 @@
 		uint32_t offset = SUNXI_SCP_BASE - vector;
 
 		mmio_write_32(vector, offset >> 2);
-		clean_dcache_range(vector, sizeof(uint32_t));
 	}
 
 	/* Take the SCP out of reset. */
diff --git a/plat/allwinner/sun50i_a64/sunxi_power.c b/plat/allwinner/sun50i_a64/sunxi_power.c
index 0fdb62d..a35b9dd 100644
--- a/plat/allwinner/sun50i_a64/sunxi_power.c
+++ b/plat/allwinner/sun50i_a64/sunxi_power.c
@@ -244,7 +244,6 @@
 	 * in instruction granularity (32 bits).
 	 */
 	mmio_write_32(arisc_reset_vec, ((uintptr_t)code - arisc_reset_vec) / 4);
-	clean_dcache_range(arisc_reset_vec, 4);
 
 	/* De-assert the arisc reset line to let it run. */
 	mmio_setbits_32(SUNXI_R_CPUCFG_BASE, BIT(0));
diff --git a/plat/arm/board/common/rotpk/arm_dev_rotpk.S b/plat/arm/board/common/rotpk/arm_dev_rotpk.S
index 80f2192..38f91fe 100644
--- a/plat/arm/board/common/rotpk/arm_dev_rotpk.S
+++ b/plat/arm/board/common/rotpk/arm_dev_rotpk.S
@@ -1,10 +1,17 @@
 /*
- * Copyright (c) 2020, ARM Limited. All rights reserved.
+ * Copyright (c) 2021, ARM Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
+/* diphda platform provides custom values for the macros defined in
+ * arm_def.h , so only platform_def.h needs to be included
+ */
+#if !defined(TARGET_PLATFORM_FVP) && !defined(TARGET_PLATFORM_FPGA)
 #include "plat/arm/common/arm_def.h"
+#else
+#include <platform_def.h>
+#endif
 
 	.global arm_rotpk_header
 	.global arm_rotpk_header_end
diff --git a/plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c b/plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c
new file mode 100644
index 0000000..916c868
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c
@@ -0,0 +1,86 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/desc_image_load.h>
+
+#include <platform_def.h>
+
+/*******************************************************************************
+ * Following descriptor provides BL image/ep information that gets used
+ * by BL2 to load the images and also subset of this information is
+ * passed to next BL image. The image loading sequence is managed by
+ * populating the images in required loading order. The image execution
+ * sequence is managed by populating the `next_handoff_image_id` with
+ * the next executable image id.
+ ******************************************************************************/
+static bl_mem_params_node_t bl2_mem_params_descs[] = {
+
+	/* Fill BL31 related information */
+	{
+		.image_id = BL31_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t,
+			SECURE | EXECUTABLE | EP_FIRST_EXE),
+		.ep_info.pc = BL31_BASE,
+		.ep_info.spsr = SPSR_64(MODE_EL3, MODE_SP_ELX,
+			DISABLE_ALL_EXCEPTIONS),
+			.ep_info.args.arg3 = ARM_BL31_PLAT_PARAM_VAL,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, IMAGE_ATTRIB_PLAT_SETUP),
+		.image_info.image_base = BL31_BASE,
+		.image_info.image_max_size = BL31_LIMIT - BL31_BASE,
+
+		.next_handoff_image_id = BL32_IMAGE_ID,
+	},
+
+	/* Fill BL32 related information */
+	{
+		.image_id = BL32_IMAGE_ID,
+
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, SECURE | EXECUTABLE),
+		.ep_info.pc = BL32_BASE,
+			.ep_info.args.arg0 = DIPHDA_TOS_FW_CONFIG_BASE,
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = BL32_BASE,
+		.image_info.image_max_size = BL32_LIMIT - BL32_BASE,
+
+		.next_handoff_image_id = BL33_IMAGE_ID,
+	},
+
+	/* Fill TOS_FW_CONFIG related information */
+	{
+		.image_id = TOS_FW_CONFIG_ID,
+		.image_info.image_base = DIPHDA_TOS_FW_CONFIG_BASE,
+		.image_info.image_max_size = DIPHDA_TOS_FW_CONFIG_LIMIT - \
+			DIPHDA_TOS_FW_CONFIG_BASE,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_IMAGE_BINARY,
+			VERSION_2, entry_point_info_t, SECURE | NON_EXECUTABLE),
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_IMAGE_BINARY,
+		VERSION_2, image_info_t, 0),
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+
+	/* Fill BL33 related information */
+	{
+		.image_id = BL33_IMAGE_ID,
+		SET_STATIC_PARAM_HEAD(ep_info, PARAM_EP,
+			VERSION_2, entry_point_info_t, NON_SECURE | EXECUTABLE),
+		.ep_info.pc = PLAT_ARM_NS_IMAGE_BASE,
+
+		SET_STATIC_PARAM_HEAD(image_info, PARAM_EP,
+			VERSION_2, image_info_t, 0),
+		.image_info.image_base = PLAT_ARM_NS_IMAGE_BASE,
+		.image_info.image_max_size = ARM_DRAM1_BASE + ARM_DRAM1_SIZE
+			- PLAT_ARM_NS_IMAGE_BASE,
+
+		.next_handoff_image_id = INVALID_IMAGE_ID,
+	},
+};
+
+REGISTER_BL_IMAGE_DESCS(bl2_mem_params_descs)
diff --git a/plat/arm/board/diphda/common/diphda_err.c b/plat/arm/board/diphda/common/diphda_err.c
new file mode 100644
index 0000000..89a3b82
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_err.c
@@ -0,0 +1,17 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * diphda error handler
+ */
+void __dead2 plat_arm_error_handler(int err)
+{
+	while (1) {
+		wfi();
+	}
+}
diff --git a/plat/arm/board/diphda/common/diphda_helpers.S b/plat/arm/board/diphda/common/diphda_helpers.S
new file mode 100644
index 0000000..c9d2a88
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_helpers.S
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <platform_def.h>
+
+	.globl	plat_secondary_cold_boot_setup
+	.globl	plat_get_my_entrypoint
+	.globl	plat_is_my_cpu_primary
+	.globl	plat_arm_calc_core_pos
+
+	/* --------------------------------------------------------------------
+	 * void plat_secondary_cold_boot_setup (void);
+	 *
+	 * For AArch32, cold-booting secondary CPUs is not yet
+	 * implemented and they panic.
+	 * --------------------------------------------------------------------
+	 */
+func plat_secondary_cold_boot_setup
+cb_panic:
+	b	cb_panic
+endfunc plat_secondary_cold_boot_setup
+
+	/* ---------------------------------------------------------------------
+	 * unsigned long plat_get_my_entrypoint (void);
+	 *
+	 * Main job of this routine is to distinguish between a cold and warm
+	 * boot. On diphda, this information can be queried from the power
+	 * controller. The Power Control SYS Status Register (PSYSR) indicates
+	 * the wake-up reason for the CPU.
+	 *
+	 * For a cold boot, return 0.
+	 * For a warm boot, Not yet supported.
+	 *
+	 * TODO: PSYSR is a common register and should be
+	 * 	accessed using locks. Since it is not possible
+	 * 	to use locks immediately after a cold reset
+	 * 	we are relying on the fact that after a cold
+	 * 	reset all cpus will read the same WK field
+	 * ---------------------------------------------------------------------
+	 */
+func plat_get_my_entrypoint
+	/* TODO support warm boot */
+	/* Cold reset */
+	mov	x0, #0
+	ret
+endfunc plat_get_my_entrypoint
+
+	/* -----------------------------------------------------
+	 * unsigned int plat_is_my_cpu_primary (void);
+	 *
+	 * Find out whether the current CPU is the primary
+	 * CPU.
+	 * -----------------------------------------------------
+	 */
+func plat_is_my_cpu_primary
+	mrs	x0, mpidr_el1
+	mov_imm	x1, MPIDR_AFFINITY_MASK
+	and	x0, x0, x1
+	cmp	x0, #DIPHDA_PRIMARY_CPU
+	cset	w0, eq
+	ret
+endfunc plat_is_my_cpu_primary
diff --git a/plat/arm/board/diphda/common/diphda_plat.c b/plat/arm/board/diphda/common/diphda_plat.c
new file mode 100644
index 0000000..28d15a5
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_plat.c
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/bl_common.h>
+
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+/*
+ * Table of regions to map using the MMU.
+ * Replace or extend the below regions as required
+ */
+
+const mmap_region_t plat_arm_mmap[] = {
+	ARM_MAP_SHARED_RAM,
+	ARM_MAP_NS_SHARED_RAM,
+	ARM_MAP_NS_DRAM1,
+	DIPHDA_MAP_DEVICE,
+	DIPHDA_EXTERNAL_FLASH,
+	{0}
+};
+
+/* diphda only has one always-on power domain and there
+ * is no power control present
+ */
+void __init plat_arm_pwrc_setup(void)
+{
+}
+
+unsigned int plat_get_syscnt_freq2(void)
+{
+	/* Returning the Generic Timer Frequency */
+	return SYS_COUNTER_FREQ_IN_TICKS;
+}
+
+
+/*
+ * Helper function to initialize ARM interconnect driver.
+ */
+void plat_arm_interconnect_init(void)
+{
+}
+
+/*
+ * Helper function to place current master into coherency
+ */
+void plat_arm_interconnect_enter_coherency(void)
+{
+}
+
+/*
+ * Helper function to remove current master from coherency
+ */
+void plat_arm_interconnect_exit_coherency(void)
+{
+}
+
+/*
+ * This function is invoked during Mbed TLS library initialisation to get a heap
+ * The function simply returns the default allocated heap.
+ */
+
+#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
diff --git a/plat/arm/board/diphda/common/diphda_pm.c b/plat/arm/board/diphda/common/diphda_pm.c
new file mode 100644
index 0000000..12b322e
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_pm.c
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <lib/psci/psci.h>
+#include <plat/arm/common/plat_arm.h>
+
+/*******************************************************************************
+ * Export the platform handlers via plat_arm_psci_pm_ops. The ARM Standard
+ * platform layer will take care of registering the handlers with PSCI.
+ ******************************************************************************/
+plat_psci_ops_t plat_arm_psci_pm_ops = {
+	/* dummy struct */
+	.validate_ns_entrypoint = NULL
+};
+
+const plat_psci_ops_t *plat_arm_psci_override_pm_ops(plat_psci_ops_t *ops)
+{
+	return ops;
+}
diff --git a/plat/arm/board/diphda/common/diphda_security.c b/plat/arm/board/diphda/common/diphda_security.c
new file mode 100644
index 0000000..bf172af
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_security.c
@@ -0,0 +1,16 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * We assume that all security programming is done by the primary core.
+ */
+void plat_arm_security_setup(void)
+{
+	/*
+	 * If the platform had additional peripheral specific security
+	 * configurations, those would be configured here.
+	 */
+}
diff --git a/plat/arm/board/diphda/common/diphda_stack_protector.c b/plat/arm/board/diphda/common/diphda_stack_protector.c
new file mode 100644
index 0000000..6228b63
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_stack_protector.c
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <plat/common/platform.h>
+
+static uint32_t plat_generate_random_number(void)
+{
+	uintptr_t return_addr = (uintptr_t)__builtin_return_address(0U);
+	uintptr_t frame_addr = (uintptr_t)__builtin_frame_address(0U);
+	uint64_t cntpct = read_cntpct_el0();
+
+	/* Generate 32-bit pattern: saving the 2 least significant bytes
+	 * in random_lo and random_hi
+	 */
+	uint16_t random_lo = (uint16_t)(
+			(((uint64_t)return_addr) << 13) ^ frame_addr ^ cntpct
+			);
+
+	uint16_t random_hi = (uint16_t)(
+			(((uint64_t)frame_addr) << 15) ^ return_addr ^ cntpct
+			);
+
+	return (((uint32_t)random_hi) << 16) | random_lo;
+}
+
+u_register_t plat_get_stack_protector_canary(void)
+{
+	return  plat_generate_random_number(); /* a 32-bit pattern returned */
+}
diff --git a/plat/arm/board/diphda/common/diphda_topology.c b/plat/arm/board/diphda/common/diphda_topology.c
new file mode 100644
index 0000000..9dfd05d
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_topology.c
@@ -0,0 +1,43 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+#include <plat/common/platform.h>
+
+/* The diphda power domain tree descriptor */
+static unsigned char diphda_power_domain_tree_desc[PLAT_ARM_CLUSTER_COUNT
+							+ 2];
+/*******************************************************************************
+ * This function dynamically constructs the topology according to
+ * CLUSTER_COUNT and returns it.
+ ******************************************************************************/
+const unsigned char *plat_get_power_domain_tree_desc(void)
+{
+	int i;
+
+	/*
+	 * The highest level is the system level. The next level is constituted
+	 * by clusters and then cores in clusters.
+	 */
+	diphda_power_domain_tree_desc[0] = 1;
+	diphda_power_domain_tree_desc[1] = PLAT_ARM_CLUSTER_COUNT;
+
+	for (i = 0; i < PLAT_ARM_CLUSTER_COUNT; i++)
+		diphda_power_domain_tree_desc[i + 2] = PLATFORM_CORE_COUNT;
+
+	return diphda_power_domain_tree_desc;
+}
+
+/******************************************************************************
+ * This function implements a part of the critical interface between the PSCI
+ * generic layer and the platform that allows the former to query the platform
+ * to convert an MPIDR to a unique linear index. An error code (-1) is
+ * returned in case the MPIDR is invalid.
+ *****************************************************************************/
+int plat_core_pos_by_mpidr(u_register_t mpidr)
+{
+	return plat_arm_calc_core_pos(mpidr);
+}
diff --git a/plat/arm/board/diphda/common/diphda_trusted_boot.c b/plat/arm/board/diphda/common/diphda_trusted_boot.c
new file mode 100644
index 0000000..ddb41fa
--- /dev/null
+++ b/plat/arm/board/diphda/common/diphda_trusted_boot.c
@@ -0,0 +1,53 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <plat/arm/common/plat_arm.h>
+
+/*
+ * 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
+ * }
+ *
+ * The function returns 0 on success. Any other value is treated as error by the
+ * Trusted Board Boot. The function also reports extra information related
+ * to the ROTPK in the flags parameter: ROTPK_IS_HASH, ROTPK_NOT_DEPLOYED.
+ *
+ * Refer to the TF-A porting-guide document for more details.
+ */
+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);
+}
+
+/*
+ * STUB overriding the non-volatile counter reading.
+ * NV counters are not implemented at this stage of development.
+ * Return: 0 = success
+ */
+int plat_get_nv_ctr(void *cookie, unsigned int *nv_ctr)
+{
+    *nv_ctr = DIPHDA_FW_NVCTR_VAL;
+    return 0;
+}
+
+/*
+ * STUB overriding the non-volatile counter updating.
+ * NV counters are not implemented at this stage of development.
+ * Return: 0 = success
+ */
+int plat_set_nv_ctr(void *cookie, unsigned int nv_ctr)
+{
+    return 0;
+}
diff --git a/plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts b/plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts
new file mode 100644
index 0000000..def04cd
--- /dev/null
+++ b/plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts
@@ -0,0 +1,30 @@
+/*
+ * Copyright (c) 2021, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+/dts-v1/;
+
+/ {
+	compatible = "arm,ffa-core-manifest-1.0";
+	#address-cells = <2>;
+	#size-cells = <1>;
+
+	/*
+	 * BL32 image details needed by SPMC
+	 *
+	 * Note:
+	 * binary_size: size of BL32 + TOS_FW_CONFIG
+	 */
+
+	attribute {
+		spmc_id = <0x8000>;
+		maj_ver = <0x1>;
+		min_ver = <0x0>;
+		exec_state = <0x0>;
+		load_address = <0x0 0x2002000>;
+		entrypoint = <0x0 0x2002000>;
+		binary_size = <0xae000>;
+	};
+
+};
diff --git a/plat/arm/board/diphda/common/include/platform_def.h b/plat/arm/board/diphda/common/include/platform_def.h
new file mode 100644
index 0000000..37fd71b
--- /dev/null
+++ b/plat/arm/board/diphda/common/include/platform_def.h
@@ -0,0 +1,416 @@
+/*
+ * Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef PLATFORM_DEF_H
+#define PLATFORM_DEF_H
+
+#include <common/tbbr/tbbr_img_def.h>
+#include <lib/utils_def.h>
+#include <lib/xlat_tables/xlat_tables_defs.h>
+#include <plat/arm/board/common/v2m_def.h>
+#include <plat/arm/common/arm_spm_def.h>
+#include <plat/arm/common/smccc_def.h>
+#include <plat/common/common_def.h>
+#include <plat/arm/soc/common/soc_css_def.h>
+
+#define ARM_ROTPK_HEADER_LEN			19
+#define ARM_ROTPK_HASH_LEN			32
+
+/* Special value used to verify platform parameters from BL2 to BL31 */
+#define ARM_BL31_PLAT_PARAM_VAL		ULL(0x0f1e2d3c4b5a6978)
+
+/* PL011 UART related constants */
+#ifdef V2M_IOFPGA_UART0_CLK_IN_HZ
+#undef V2M_IOFPGA_UART0_CLK_IN_HZ
+#endif
+
+#ifdef V2M_IOFPGA_UART1_CLK_IN_HZ
+#undef V2M_IOFPGA_UART1_CLK_IN_HZ
+#endif
+
+#define V2M_IOFPGA_UART0_CLK_IN_HZ		50000000
+#define V2M_IOFPGA_UART1_CLK_IN_HZ		50000000
+
+/* Core/Cluster/Thread counts for diphda */
+#define DIPHDA_CLUSTER_COUNT			U(1)
+#define DIPHDA_MAX_CPUS_PER_CLUSTER		U(4)
+#define DIPHDA_MAX_PE_PER_CPU			U(1)
+#define DIPHDA_PRIMARY_CPU			U(0)
+
+#define PLAT_ARM_CLUSTER_COUNT		DIPHDA_CLUSTER_COUNT
+
+#define PLATFORM_CORE_COUNT			(PLAT_ARM_CLUSTER_COUNT *      \
+						DIPHDA_MAX_CPUS_PER_CLUSTER *  \
+						DIPHDA_MAX_PE_PER_CPU)
+
+/* UART related constants */
+#define PLAT_ARM_BOOT_UART_BASE		0x1a510000
+#define PLAT_ARM_BOOT_UART_CLK_IN_HZ		V2M_IOFPGA_UART0_CLK_IN_HZ
+#define PLAT_ARM_RUN_UART_BASE		0x1a520000
+#define PLAT_ARM_RUN_UART_CLK_IN_HZ		V2M_IOFPGA_UART1_CLK_IN_HZ
+#define ARM_CONSOLE_BAUDRATE			115200
+#define PLAT_ARM_CRASH_UART_BASE		PLAT_ARM_RUN_UART_BASE
+#define PLAT_ARM_CRASH_UART_CLK_IN_HZ		PLAT_ARM_RUN_UART_CLK_IN_HZ
+
+/* Memory related constants */
+
+/* SRAM (CVM) memory layout
+ *
+ * <ARM_TRUSTED_SRAM_BASE>
+ *
+ *         partition size: sizeof(meminfo_t) = 16 bytes
+ *
+ *         content: memory info area used by the next BL
+ *
+ * <ARM_FW_CONFIG_BASE>
+ *
+ *         partition size: 4080 bytes
+ *
+ * <ARM_BL2_MEM_DESC_BASE>
+ *
+ *         partition size: 4 KB
+ *
+ *         content:
+ *
+ *             Area where BL2 copies the images descriptors
+ *
+ * <ARM_BL_RAM_BASE> = <BL32_BASE>
+ *
+ *         partition size: 688 KB
+ *
+ *         content:
+ *
+ *             BL32 (optee-os)
+ *
+ * <DIPHDA_TOS_FW_CONFIG_BASE> = 0x20ae000
+ *
+ *         partition size: 8 KB
+ *
+ *         content:
+ *
+ *             BL32 config (TOS_FW_CONFIG)
+ *
+ * <BL31_BASE>
+ *
+ *         partition size: 140 KB
+ *
+ *         content:
+ *
+ *             BL31
+ *
+ * <BL2_SIGNATURE_BASE>
+ *
+ *     partition size: 4 KB
+ *
+ *     content:
+ *
+ *         MCUBOOT data needed to verify TF-A BL2
+ *
+ * <BL2_BASE>
+ *
+ *     partition size: 176 KB
+ *
+ *         content:
+ *
+ *             BL2
+ *
+ * <ARM_NS_SHARED_RAM_BASE> = <ARM_TRUSTED_SRAM_BASE> + 1 MB
+ *
+ *         partition size: 3 MB
+ *
+ *         content:
+ *
+ *             BL33 (u-boot)
+ */
+
+/* DDR memory */
+#define ARM_DRAM1_BASE			UL(0x80000000)
+#define ARM_DRAM1_SIZE			UL(0x80000000)
+#define ARM_DRAM1_END				(ARM_DRAM1_BASE +	\
+						ARM_DRAM1_SIZE - 1)
+
+/* DRAM1 and DRAM2 are the same for diphda */
+#define ARM_DRAM2_BASE			ARM_DRAM1_BASE
+#define ARM_DRAM2_SIZE			ARM_DRAM1_SIZE
+#define ARM_DRAM2_END				ARM_DRAM1_END
+
+#define ARM_NS_DRAM1_BASE			ARM_DRAM1_BASE
+#define ARM_NS_DRAM1_SIZE			ARM_DRAM1_SIZE
+#define ARM_NS_DRAM1_END			(ARM_NS_DRAM1_BASE +	\
+						ARM_NS_DRAM1_SIZE - 1)
+
+/* The first 8 KB of Trusted SRAM are used as shared memory */
+#define ARM_TRUSTED_SRAM_BASE			UL(0x02000000)
+#define ARM_SHARED_RAM_SIZE			UL(0x00002000)  /* 8 KB */
+#define ARM_SHARED_RAM_BASE			ARM_TRUSTED_SRAM_BASE
+
+/* The remaining Trusted SRAM is used to load the BL images */
+
+#define PLAT_ARM_TRUSTED_SRAM_SIZE		UL(0x00100000)  /* 1 MB */
+
+#define PLAT_ARM_MAX_BL2_SIZE			UL(0x0002d000)  /* 180 KB */
+
+#define PLAT_ARM_MAX_BL31_SIZE		UL(0x00023000)  /* 140 KB */
+
+#define ARM_BL_RAM_BASE			(ARM_SHARED_RAM_BASE +	\
+						ARM_SHARED_RAM_SIZE)
+#define ARM_BL_RAM_SIZE			(PLAT_ARM_TRUSTED_SRAM_SIZE -	\
+						ARM_SHARED_RAM_SIZE)
+
+#define BL2_SIGNATURE_SIZE			UL(0x00001000)  /* 4 KB */
+#define BL2_SIGNATURE_BASE			(BL2_LIMIT - \
+						PLAT_ARM_MAX_BL2_SIZE)
+#define BL2_BASE				(BL2_LIMIT - \
+						PLAT_ARM_MAX_BL2_SIZE + \
+						BL2_SIGNATURE_SIZE)
+#define BL2_LIMIT				(ARM_BL_RAM_BASE + \
+						ARM_BL_RAM_SIZE)
+
+#define BL31_BASE				(BL2_SIGNATURE_BASE - \
+						PLAT_ARM_MAX_BL31_SIZE)
+#define BL31_LIMIT				BL2_SIGNATURE_BASE
+
+#define DIPHDA_TOS_FW_CONFIG_BASE		(BL31_BASE - \
+						DIPHDA_TOS_FW_CONFIG_SIZE)
+#define DIPHDA_TOS_FW_CONFIG_SIZE		UL(0x00002000)  /* 8 KB */
+#define DIPHDA_TOS_FW_CONFIG_LIMIT		BL31_BASE
+
+#define BL32_BASE				ARM_BL_RAM_BASE
+#define PLAT_ARM_MAX_BL32_SIZE		(DIPHDA_TOS_FW_CONFIG_BASE - \
+						BL32_BASE)     /* 688 KB */
+#define BL32_LIMIT				(BL32_BASE + \
+						PLAT_ARM_MAX_BL32_SIZE)
+
+/* SPD_spmd settings */
+
+#define PLAT_ARM_SPMC_BASE			BL32_BASE
+#define PLAT_ARM_SPMC_SIZE			PLAT_ARM_MAX_BL32_SIZE
+
+/* NS memory */
+
+/* The last 3 MB of the SRAM is allocated to the non secure area */
+#define ARM_NS_SHARED_RAM_BASE		(ARM_TRUSTED_SRAM_BASE + \
+						PLAT_ARM_TRUSTED_SRAM_SIZE)
+#define ARM_NS_SHARED_RAM_SIZE		UL(0x00300000)  /* 3 MB */
+
+/* end of the definition of SRAM memory layout */
+
+/* NOR Flash */
+
+#define PLAT_ARM_FIP_BASE			UL(0x08131000)
+#define PLAT_ARM_FIP_MAX_SIZE			UL(0x1ff000)  /* 1.996 MB */
+
+#define PLAT_ARM_NVM_BASE			V2M_FLASH0_BASE
+#define PLAT_ARM_NVM_SIZE			UL(0x02000000)  /* 32 MB */
+
+#define PLAT_ARM_FLASH_IMAGE_BASE		PLAT_ARM_FIP_BASE
+#define PLAT_ARM_FLASH_IMAGE_MAX_SIZE		PLAT_ARM_FIP_MAX_SIZE
+
+/*
+ * Some data must be aligned on the biggest cache line size in the platform.
+ * This is known only to the platform as it might have a combination of
+ * integrated and external caches.
+ */
+#define CACHE_WRITEBACK_GRANULE		(U(1) << ARM_CACHE_WRITEBACK_SHIFT)
+#define ARM_CACHE_WRITEBACK_SHIFT		6
+
+/*
+ * Define FW_CONFIG area base and limit. Leave enough space for BL2 meminfo.
+ * FW_CONFIG is intended to host the device tree. Currently, This area is not
+ * used because diphda platform doesn't use a device tree at TF-A level.
+ */
+#define ARM_FW_CONFIG_BASE			(ARM_SHARED_RAM_BASE \
+						+ sizeof(meminfo_t))
+#define ARM_FW_CONFIG_LIMIT			(ARM_SHARED_RAM_BASE \
+						+ (ARM_SHARED_RAM_SIZE >> 1))
+
+/*
+ * Boot parameters passed from BL2 to BL31/BL32 are stored here
+ */
+#define ARM_BL2_MEM_DESC_BASE			ARM_FW_CONFIG_LIMIT
+#define ARM_BL2_MEM_DESC_LIMIT		ARM_BL_RAM_BASE
+
+/*
+ * The max number of regions like RO(code), coherent and data required by
+ * different BL stages which need to be mapped in the MMU.
+ */
+#define ARM_BL_REGIONS			3
+#define PLAT_ARM_MMAP_ENTRIES			8
+#define MAX_XLAT_TABLES			5
+#define MAX_MMAP_REGIONS			(PLAT_ARM_MMAP_ENTRIES + \
+						ARM_BL_REGIONS)
+#define MAX_IO_DEVICES			2
+#define MAX_IO_HANDLES			3
+#define MAX_IO_BLOCK_DEVICES			1
+
+/* GIC related constants */
+#define PLAT_ARM_GICD_BASE			0x1C010000
+#define PLAT_ARM_GICC_BASE			0x1C02F000
+
+/* MHUv2 Secure Channel receiver and sender */
+#define PLAT_SDK700_MHU0_SEND			0x1B800000
+#define PLAT_SDK700_MHU0_RECV			0x1B810000
+
+/* Timer/watchdog related constants */
+#define ARM_SYS_CNTCTL_BASE			UL(0x1a200000)
+#define ARM_SYS_CNTREAD_BASE			UL(0x1a210000)
+#define ARM_SYS_TIMCTL_BASE			UL(0x1a220000)
+
+#define SYS_COUNTER_FREQ_IN_TICKS	UL(50000000) /* 50MHz */
+
+#define DIPHDA_IRQ_TZ_WDOG			32
+#define DIPHDA_IRQ_SEC_SYS_TIMER		34
+
+#define PLAT_MAX_PWR_LVL			2
+/*
+ * Macros mapping the MPIDR Affinity levels to ARM Platform Power levels. The
+ * power levels have a 1:1 mapping with the MPIDR affinity levels.
+ */
+#define ARM_PWR_LVL0				MPIDR_AFFLVL0
+#define ARM_PWR_LVL1				MPIDR_AFFLVL1
+#define ARM_PWR_LVL2				MPIDR_AFFLVL2
+
+/*
+ *  Macros for local power states in ARM platforms encoded by State-ID field
+ *  within the power-state parameter.
+ */
+/* Local power state for power domains in Run state. */
+#define ARM_LOCAL_STATE_RUN			U(0)
+/* Local power state for retention. Valid only for CPU power domains */
+#define ARM_LOCAL_STATE_RET			U(1)
+/* Local power state for OFF/power-down. Valid for CPU and cluster
+ * power domains
+ */
+#define ARM_LOCAL_STATE_OFF			U(2)
+
+#define PLAT_ARM_TRUSTED_MAILBOX_BASE		ARM_TRUSTED_SRAM_BASE
+#define PLAT_ARM_NSTIMER_FRAME_ID		U(1)
+
+#define PLAT_ARM_NS_IMAGE_BASE		(ARM_NS_SHARED_RAM_BASE)
+
+#define PLAT_PHY_ADDR_SPACE_SIZE		(1ULL << 32)
+#define PLAT_VIRT_ADDR_SPACE_SIZE		(1ULL << 32)
+
+/*
+ * This macro defines the deepest retention state possible. A higher state
+ * ID will represent an invalid or a power down state.
+ */
+#define PLAT_MAX_RET_STATE			1
+
+/*
+ * This macro defines the deepest power down states possible. Any state ID
+ * higher than this is invalid.
+ */
+#define PLAT_MAX_OFF_STATE			2
+
+#define PLATFORM_STACK_SIZE			UL(0x440)
+
+#define DIPHDA_EXTERNAL_FLASH			MAP_REGION_FLAT(	\
+						PLAT_ARM_NVM_BASE,	\
+						PLAT_ARM_NVM_SIZE,	\
+						MT_DEVICE | MT_RO | MT_SECURE)
+
+#define ARM_MAP_SHARED_RAM			MAP_REGION_FLAT(	\
+						ARM_SHARED_RAM_BASE,	\
+						ARM_SHARED_RAM_SIZE,	\
+						MT_MEMORY | MT_RW | MT_SECURE)
+
+#define ARM_MAP_NS_SHARED_RAM			MAP_REGION_FLAT(	\
+						ARM_NS_SHARED_RAM_BASE, \
+						ARM_NS_SHARED_RAM_SIZE, \
+						MT_MEMORY | MT_RW | MT_NS)
+
+#define ARM_MAP_NS_DRAM1			MAP_REGION_FLAT(	\
+						ARM_NS_DRAM1_BASE,	\
+						ARM_NS_DRAM1_SIZE,	\
+						MT_MEMORY | MT_RW | MT_NS)
+
+#define ARM_MAP_BL_RO				MAP_REGION_FLAT(	\
+						BL_CODE_BASE,		\
+						BL_CODE_END		\
+							- BL_CODE_BASE, \
+						MT_CODE | MT_SECURE),	\
+						MAP_REGION_FLAT(	\
+						BL_RO_DATA_BASE,	\
+						BL_RO_DATA_END	\
+						- BL_RO_DATA_BASE,	\
+						MT_RO_DATA | MT_SECURE)
+#if USE_COHERENT_MEM
+#define ARM_MAP_BL_COHERENT_RAM		MAP_REGION_FLAT(	\
+						BL_COHERENT_RAM_BASE,	\
+						BL_COHERENT_RAM_END	\
+						- BL_COHERENT_RAM_BASE, \
+						MT_DEVICE | MT_RW | MT_SECURE)
+#endif
+
+/*
+ * Map the region for the optional device tree configuration with read and
+ * write permissions
+ */
+#define ARM_MAP_BL_CONFIG_REGION		MAP_REGION_FLAT(	\
+						ARM_FW_CONFIG_BASE,	\
+						(ARM_FW_CONFIG_LIMIT-   \
+						ARM_FW_CONFIG_BASE),   \
+						MT_MEMORY | MT_RW | MT_SECURE)
+
+#define DIPHDA_DEVICE_BASE			(0x1A000000)
+#define DIPHDA_DEVICE_SIZE			(0x26000000)
+#define DIPHDA_MAP_DEVICE			MAP_REGION_FLAT(	\
+						DIPHDA_DEVICE_BASE,	\
+						DIPHDA_DEVICE_SIZE,	\
+						MT_DEVICE | MT_RW | MT_SECURE)
+
+#define ARM_IRQ_SEC_PHY_TIMER			29
+
+#define ARM_IRQ_SEC_SGI_0			8
+#define ARM_IRQ_SEC_SGI_1			9
+#define ARM_IRQ_SEC_SGI_2			10
+#define ARM_IRQ_SEC_SGI_3			11
+#define ARM_IRQ_SEC_SGI_4			12
+#define ARM_IRQ_SEC_SGI_5			13
+#define ARM_IRQ_SEC_SGI_6			14
+#define ARM_IRQ_SEC_SGI_7			15
+
+/*
+ * Define a list of Group 1 Secure and Group 0 interrupt properties as per GICv3
+ * terminology. On a GICv2 system or mode, the lists will be merged and treated
+ * as Group 0 interrupts.
+ */
+#define ARM_G1S_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(ARM_IRQ_SEC_PHY_TIMER, GIC_HIGHEST_SEC_PRIORITY, \
+		(grp), GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_1, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_2, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_3, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_4, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_5, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE), \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_7, GIC_HIGHEST_SEC_PRIORITY,	\
+		(grp), GIC_INTR_CFG_EDGE)
+
+#define ARM_G0_IRQ_PROPS(grp) \
+	INTR_PROP_DESC(ARM_IRQ_SEC_SGI_6, GIC_HIGHEST_SEC_PRIORITY, (grp), \
+		GIC_INTR_CFG_EDGE)
+
+/*
+ * Define a list of Group 1 Secure and Group 0 interrupts as per GICv3
+ * terminology. On a GICv2 system or mode, the lists will be merged and treated
+ * as Group 0 interrupts.
+ */
+#define PLAT_ARM_G1S_IRQ_PROPS(grp)	\
+	ARM_G1S_IRQ_PROPS(grp), \
+	INTR_PROP_DESC(DIPHDA_IRQ_TZ_WDOG, GIC_HIGHEST_SEC_PRIORITY, \
+		(grp), GIC_INTR_CFG_LEVEL), \
+	INTR_PROP_DESC(DIPHDA_IRQ_SEC_SYS_TIMER, \
+		GIC_HIGHEST_SEC_PRIORITY, (grp), GIC_INTR_CFG_LEVEL)
+
+#define PLAT_ARM_G0_IRQ_PROPS(grp)	ARM_G0_IRQ_PROPS(grp)
+
+#endif /* PLATFORM_DEF_H */
diff --git a/plat/arm/board/diphda/include/plat_macros.S b/plat/arm/board/diphda/include/plat_macros.S
new file mode 100644
index 0000000..4de8f95
--- /dev/null
+++ b/plat/arm/board/diphda/include/plat_macros.S
@@ -0,0 +1,22 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#ifndef PLAT_MACROS_S
+#define PLAT_MACROS_S
+
+#include <css_macros.S>
+
+/* ---------------------------------------------
+ * The below required platform porting macro
+ * prints out relevant platform registers
+ * whenever an unhandled exception is taken in
+ * BL31.
+ * ---------------------------------------------
+ */
+	.macro plat_crash_print_regs
+	css_print_gic_regs
+	.endm
+
+#endif /* PLAT_MACROS_S */
diff --git a/plat/arm/board/diphda/platform.mk b/plat/arm/board/diphda/platform.mk
new file mode 100644
index 0000000..5ff0862
--- /dev/null
+++ b/plat/arm/board/diphda/platform.mk
@@ -0,0 +1,76 @@
+#
+# Copyright (c) 2021, Arm Limited and Contributors. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Making sure the diphda platform type is specified
+ifeq ($(filter ${TARGET_PLATFORM}, fpga fvp),)
+	$(error TARGET_PLATFORM must be fpga or fvp)
+endif
+
+DIPHDA_CPU_LIBS	+=lib/cpus/aarch64/cortex_a35.S
+
+PLAT_INCLUDES		:=	-Iplat/arm/board/diphda/common/include	\
+				-Iplat/arm/board/diphda/include		\
+				-Iinclude/plat/arm/common			\
+				-Iinclude/plat/arm/css/common/aarch64
+
+
+DIPHDA_FW_NVCTR_VAL	:=	255
+TFW_NVCTR_VAL		:=	${DIPHDA_FW_NVCTR_VAL}
+NTFW_NVCTR_VAL		:=	${DIPHDA_FW_NVCTR_VAL}
+
+override NEED_BL1	:=	no
+
+override NEED_BL2	:=	yes
+FIP_BL2_ARGS := tb-fw
+
+override NEED_BL2U	:=	no
+override NEED_BL31	:=	yes
+NEED_BL32		:=	yes
+override NEED_BL33	:=	yes
+
+# Include GICv2 driver files
+include drivers/arm/gic/v2/gicv2.mk
+
+DIPHDA_GIC_SOURCES	:=	${GICV2_SOURCES}			\
+				plat/common/plat_gicv2.c		\
+				plat/arm/common/arm_gicv2.c
+
+
+BL2_SOURCES		+=	plat/arm/board/diphda/common/diphda_security.c		\
+				plat/arm/board/diphda/common/diphda_err.c		\
+				plat/arm/board/diphda/common/diphda_trusted_boot.c	\
+				lib/utils/mem_region.c					\
+				plat/arm/board/diphda/common/diphda_helpers.S		\
+				plat/arm/board/diphda/common/diphda_plat.c		\
+				plat/arm/board/diphda/common/diphda_bl2_mem_params_desc.c \
+				${DIPHDA_CPU_LIBS}					\
+
+
+BL31_SOURCES	+=	drivers/cfi/v2m/v2m_flash.c				\
+			lib/utils/mem_region.c					\
+			plat/arm/board/diphda/common/diphda_helpers.S		\
+			plat/arm/board/diphda/common/diphda_topology.c		\
+			plat/arm/board/diphda/common/diphda_security.c		\
+			plat/arm/board/diphda/common/diphda_plat.c		\
+			plat/arm/board/diphda/common/diphda_pm.c		\
+			${DIPHDA_CPU_LIBS}					\
+			${DIPHDA_GIC_SOURCES}
+
+
+FDT_SOURCES		+=	plat/arm/board/diphda/common/fdts/diphda_spmc_manifest.dts
+DIPHDA_TOS_FW_CONFIG	:=	${BUILD_PLAT}/fdts/diphda_spmc_manifest.dtb
+
+# Add the SPMC manifest to FIP and specify the same to certtool
+$(eval $(call TOOL_ADD_PAYLOAD,${DIPHDA_TOS_FW_CONFIG},--tos-fw-config,${DIPHDA_TOS_FW_CONFIG}))
+
+# Adding TARGET_PLATFORM as a GCC define (-D option)
+$(eval $(call add_define,TARGET_PLATFORM_$(call uppercase,${TARGET_PLATFORM})))
+
+# Adding DIPHDA_FW_NVCTR_VAL as a GCC define (-D option)
+$(eval $(call add_define,DIPHDA_FW_NVCTR_VAL))
+
+include plat/arm/common/arm_common.mk
+include plat/arm/board/common/board_common.mk
diff --git a/plat/arm/board/fvp/fvp_bl1_setup.c b/plat/arm/board/fvp/fvp_bl1_setup.c
index e713bbc..06ee037 100644
--- a/plat/arm/board/fvp/fvp_bl1_setup.c
+++ b/plat/arm/board/fvp/fvp_bl1_setup.c
@@ -1,15 +1,17 @@
 /*
- * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
 
 #include <assert.h>
+#include <errno.h>
 
 #include <bl1/bl1.h>
 #include <common/tbbr/tbbr_img_def.h>
 #include <drivers/arm/smmu_v3.h>
 #include <drivers/arm/sp805.h>
+#include <lib/mmio.h>
 #include <plat/arm/common/arm_config.h>
 #include <plat/arm/common/plat_arm.h>
 #include <plat/arm/common/arm_def.h>
@@ -61,6 +63,12 @@
 
 __dead2 void bl1_plat_fwu_done(void *client_cookie, void *reserved)
 {
+	uint32_t nv_flags = mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
+
+	/* Clear the NV flags register. */
+	mmio_write_32((V2M_SYSREGS_BASE + V2M_SYS_NVFLAGSCLR),
+		      nv_flags);
+
 	/* Setup the watchdog to reset the system as soon as possible */
 	sp805_refresh(ARM_SP805_TWDG_BASE, 1U);
 
@@ -124,3 +132,15 @@
 	return 0;
 }
 #endif /* MEASURED_BOOT */
+
+/*******************************************************************************
+ * The following function checks if Firmware update is needed by checking error
+ * reported in NV flag.
+ ******************************************************************************/
+bool plat_arm_bl1_fwu_needed(void)
+{
+	int32_t nv_flags = (int32_t)mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
+
+	/* if image load/authentication failed */
+	return ((nv_flags == -EAUTH) || (nv_flags == -ENOENT));
+}
diff --git a/plat/arm/board/fvp/fvp_common.c b/plat/arm/board/fvp/fvp_common.c
index fe0903b..9d3c031 100644
--- a/plat/arm/board/fvp/fvp_common.c
+++ b/plat/arm/board/fvp/fvp_common.c
@@ -72,14 +72,11 @@
  * Table of memory regions for various BL stages to map using the MMU.
  * This doesn't include Trusted SRAM as setup_page_tables() already takes care
  * of mapping it.
- *
- * The flash needs to be mapped as writable in order to erase the FIP's Table of
- * Contents in case of unrecoverable error (see plat_error_handler()).
  */
 #ifdef IMAGE_BL1
 const mmap_region_t plat_arm_mmap[] = {
 	ARM_MAP_SHARED_RAM,
-	V2M_MAP_FLASH0_RW,
+	V2M_MAP_FLASH0_RO,
 	V2M_MAP_IOFPGA,
 	MAP_DEVICE0,
 #if FVP_INTERCONNECT_DRIVER == FVP_CCN
diff --git a/plat/arm/board/fvp/fvp_err.c b/plat/arm/board/fvp/fvp_err.c
index 2928b3a..1f9f0dd 100644
--- a/plat/arm/board/fvp/fvp_err.c
+++ b/plat/arm/board/fvp/fvp_err.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2019-2020, Arm Limited. All rights reserved.
+ * Copyright (c) 2019-2021, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -9,6 +9,7 @@
 #include <common/debug.h>
 #include <drivers/arm/sp805.h>
 #include <drivers/cfi/v2m_flash.h>
+#include <lib/mmio.h>
 #include <plat/arm/common/plat_arm.h>
 #include <platform_def.h>
 
@@ -17,25 +18,8 @@
  */
 __dead2 void plat_arm_error_handler(int err)
 {
-	int ret;
-
-	switch (err) {
-	case -ENOENT:
-	case -EAUTH:
-		/* Image load or authentication error. Erase the ToC */
-		INFO("Erasing FIP ToC from flash...\n");
-		(void)nor_unlock(PLAT_ARM_FLASH_IMAGE_BASE);
-		ret = nor_word_program(PLAT_ARM_FLASH_IMAGE_BASE, 0);
-		if (ret != 0) {
-			ERROR("Cannot erase ToC\n");
-		} else {
-			INFO("Done\n");
-		}
-		break;
-	default:
-		/* Unexpected error */
-		break;
-	}
+	/* Propagate the err code in the NV-flags register */
+	mmio_write_32(V2M_SYS_NVFLAGS_ADDR, (uint32_t)err);
 
 	console_flush();
 
diff --git a/plat/arm/board/fvp/fvp_io_storage.c b/plat/arm/board/fvp/fvp_io_storage.c
index 109d321..4eef51c 100644
--- a/plat/arm/board/fvp/fvp_io_storage.c
+++ b/plat/arm/board/fvp/fvp_io_storage.c
@@ -20,6 +20,10 @@
 #define BL32_IMAGE_NAME			"bl32.bin"
 #define BL33_IMAGE_NAME			"bl33.bin"
 #define TB_FW_CONFIG_NAME		"fvp_tb_fw_config.dtb"
+#define SOC_FW_CONFIG_NAME		"fvp_soc_fw_config.dtb"
+#define TOS_FW_CONFIG_NAME		"fvp_tsp_fw_config.dtb"
+#define NT_FW_CONFIG_NAME		"fvp_nt_fw_config.dtb"
+#define FW_CONFIG_NAME			"fvp_fw_config.dtb"
 #define HW_CONFIG_NAME			"hw_config.dtb"
 
 #if TRUSTED_BOARD_BOOT
@@ -58,6 +62,22 @@
 		.path = TB_FW_CONFIG_NAME,
 		.mode = FOPEN_MODE_RB
 	},
+	[SOC_FW_CONFIG_ID] = {
+		.path = SOC_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[TOS_FW_CONFIG_ID] = {
+		.path = TOS_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[NT_FW_CONFIG_ID] = {
+		.path = NT_FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
+	[FW_CONFIG_ID] = {
+		.path = FW_CONFIG_NAME,
+		.mode = FOPEN_MODE_RB
+	},
 	[HW_CONFIG_ID] = {
 		.path = HW_CONFIG_NAME,
 		.mode = FOPEN_MODE_RB
diff --git a/plat/arm/board/juno/juno_bl1_setup.c b/plat/arm/board/juno/juno_bl1_setup.c
index 2234055..a9d5cc3 100644
--- a/plat/arm/board/juno/juno_bl1_setup.c
+++ b/plat/arm/board/juno/juno_bl1_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -62,11 +62,11 @@
  ******************************************************************************/
 bool plat_arm_bl1_fwu_needed(void)
 {
-	const int32_t *nv_flags_ptr = (const int32_t *)V2M_SYS_NVFLAGS_ADDR;
+	int32_t nv_flags = (int32_t)mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
 
 	/* Check if TOC is invalid or watchdog reset happened. */
-	return (!arm_io_is_toc_valid() || (((*nv_flags_ptr == -EAUTH) ||
-		(*nv_flags_ptr == -ENOENT)) && is_watchdog_reset()));
+	return (!arm_io_is_toc_valid() || (((nv_flags == -EAUTH) ||
+		(nv_flags == -ENOENT)) && is_watchdog_reset()));
 }
 
 /*******************************************************************************
@@ -86,13 +86,11 @@
  ******************************************************************************/
 __dead2 void bl1_plat_fwu_done(void *client_cookie, void *reserved)
 {
-	unsigned int *nv_flags_clr = (unsigned int *)
-			(V2M_SYSREGS_BASE + V2M_SYS_NVFLAGSCLR);
-	unsigned int *nv_flags_ptr = (unsigned int *)
-			(V2M_SYSREGS_BASE + V2M_SYS_NVFLAGS);
+	uint32_t nv_flags = mmio_read_32(V2M_SYS_NVFLAGS_ADDR);
 
 	/* Clear the NV flags register. */
-	*nv_flags_clr = *nv_flags_ptr;
+	mmio_write_32((V2M_SYSREGS_BASE + V2M_SYS_NVFLAGSCLR),
+		      nv_flags);
 
 	/* Setup the watchdog to reset the system as soon as possible */
 	sp805_refresh(ARM_SP805_TWDG_BASE, 1U);
diff --git a/plat/arm/board/juno/juno_err.c b/plat/arm/board/juno/juno_err.c
index 60699cc..02d751e 100644
--- a/plat/arm/board/juno/juno_err.c
+++ b/plat/arm/board/juno/juno_err.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -16,10 +16,8 @@
  */
 void __dead2 plat_arm_error_handler(int err)
 {
-	uint32_t *flags_ptr = (uint32_t *)V2M_SYS_NVFLAGS_ADDR;
-
 	/* Propagate the err code in the NV-flags register */
-	*flags_ptr = err;
+	mmio_write_32(V2M_SYS_NVFLAGS_ADDR, (uint32_t)err);
 
 	/* Setup the watchdog to reset the system as soon as possible */
 	sp805_refresh(ARM_SP805_TWDG_BASE, 1U);
diff --git a/plat/arm/board/tc0/platform.mk b/plat/arm/board/tc0/platform.mk
index 814ccd3..5ae87d1 100644
--- a/plat/arm/board/tc0/platform.mk
+++ b/plat/arm/board/tc0/platform.mk
@@ -114,6 +114,8 @@
 
 override ENABLE_SPE_FOR_LOWER_ELS	:= 0
 
+override ENABLE_AMU := 1
+
 include plat/arm/common/arm_common.mk
 include plat/arm/css/common/css_common.mk
 include plat/arm/soc/common/soc_css.mk
diff --git a/plat/arm/common/arm_common.mk b/plat/arm/common/arm_common.mk
index f1e4cf5..de25a53 100644
--- a/plat/arm/common/arm_common.mk
+++ b/plat/arm/common/arm_common.mk
@@ -256,8 +256,10 @@
 ifeq (${JUNO_AARCH32_EL3_RUNTIME},1)
 BL2_SOURCES		+=	plat/arm/common/aarch32/arm_bl2_mem_params_desc.c
 else
+ifeq ($(filter ${TARGET_PLATFORM}, fpga fvp),)
 BL2_SOURCES		+=	plat/arm/common/${ARCH}/arm_bl2_mem_params_desc.c
 endif
+endif
 BL2_SOURCES		+=	plat/arm/common/arm_image_load.c		\
 				common/desc_image_load.c
 ifeq (${SPD},opteed)
diff --git a/plat/arm/css/sgi/include/sgi_ras.h b/plat/arm/css/sgi/include/sgi_ras.h
index 4b8a0d1..e69a684 100644
--- a/plat/arm/css/sgi/include/sgi_ras.h
+++ b/plat/arm/css/sgi/include/sgi_ras.h
@@ -7,16 +7,11 @@
 #ifndef SGI_RAS_H
 #define SGI_RAS_H
 
-/* Platform specific SMC FID's used for DMC-620 RAS error handling */
-#define SP_DMC_ERROR_OVERFLOW_EVENT_AARCH64	0xC4000043
-#define SP_DMC_ERROR_ECC_EVENT_AARCH64		0xC4000044
-
 /*
  * Mapping the RAS interrupt with SDEI event number and the event
  * id used with Standalone MM code
  */
 struct sgi_ras_ev_map {
-	int ras_ev_num;		/* RAS Event number */
 	int sdei_ev_num;	/* SDEI Event number */
 	int intr;		/* Physical intr number */
 };
diff --git a/plat/arm/css/sgi/sgi_ras.c b/plat/arm/css/sgi/sgi_ras.c
index a04972d..7aa477d 100644
--- a/plat/arm/css/sgi/sgi_ras.c
+++ b/plat/arm/css/sgi/sgi_ras.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -33,31 +33,45 @@
 	uint8_t		data[8];
 } mm_communicate_header_t;
 
+/*
+ * GUID to indicate that the MM communication message is intended for DMC-620
+ * MM driver.
+ */
+const struct efi_guid dmc620_ecc_event_guid = {
+	0x5ef0afd5, 0xe01a, 0x4c30,
+	{0x86, 0x19, 0x45, 0x46, 0x26, 0x91, 0x80, 0x98}
+};
+
 struct sgi_ras_ev_map sgi575_ras_map[] = {
 
-	/* DMC620 error overflow interrupt*/
-	{SP_DMC_ERROR_OVERFLOW_EVENT_AARCH64, SGI_SDEI_DS_EVENT_1, 33},
+	/* DMC 0 error ECC error interrupt*/
+	{SGI_SDEI_DS_EVENT_0, 35},
 
-	/* DMC620 error ECC error interrupt*/
-	{SP_DMC_ERROR_ECC_EVENT_AARCH64, SGI_SDEI_DS_EVENT_0, 35},
+	/* DMC 1 error ECC error interrupt*/
+	{SGI_SDEI_DS_EVENT_1, 39},
 };
 
 #define SGI575_RAS_MAP_SIZE	ARRAY_SIZE(sgi575_ras_map)
 
 struct err_record_info sgi_err_records[] = {
 	{
+		/* DMC 0 error record info */
 		.handler = &sgi_ras_intr_handler,
+		.aux_data = (void *)0,
+	}, {
+		/* DMC 1 error record info */
+		.handler = &sgi_ras_intr_handler,
+		.aux_data = (void *)1,
 	},
 };
 
 struct ras_interrupt sgi_ras_interrupts[] = {
 	{
-		.intr_number = 33,
-		.err_record = &sgi_err_records[0],
-	},
-	{
 		.intr_number = 35,
 		.err_record = &sgi_err_records[0],
+	}, {
+		.intr_number = 39,
+		.err_record = &sgi_err_records[1],
 	}
 };
 
@@ -138,9 +152,10 @@
 	 */
 	header = (void *) PLAT_SPM_BUF_BASE;
 	memset(header, 0, sizeof(*header));
-	memcpy(&header->data, &ras_map->ras_ev_num,
-	       sizeof(ras_map->ras_ev_num));
-	header->message_len = 4;
+	memcpy(&header->data, &err_rec->aux_data, sizeof(err_rec->aux_data));
+	header->message_len = sizeof(err_rec->aux_data);
+	memcpy(&header->header_guid, (void *) &dmc620_ecc_event_guid,
+			sizeof(const struct efi_guid));
 
 	spm_mm_sp_call(MM_COMMUNICATE_AARCH64, (uint64_t)header, 0,
 		       plat_my_core_pos());
diff --git a/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c b/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c
index 3cf5c36..2df96ae 100644
--- a/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c
+++ b/plat/imx/imx7/picopi/picopi_bl2_el3_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -43,6 +43,8 @@
 	 IOMUXC_SW_PAD_CTL_PAD_SD3_SLEW_SLOW         | \
 	 IOMUXC_SW_PAD_CTL_PAD_SD3_DSE_3_X6)
 
+static struct mmc_device_info mmc_info;
+
 static void picopi_setup_pinmux(void)
 {
 	/* Configure UART5 TX */
@@ -93,14 +95,13 @@
 static void picopi_usdhc_setup(void)
 {
 	imx_usdhc_params_t params;
-	struct mmc_device_info info;
 
 	zeromem(&params, sizeof(imx_usdhc_params_t));
 	params.reg_base = PLAT_PICOPI_BOOT_MMC_BASE;
 	params.clk_rate = 25000000;
 	params.bus_width = MMC_BUS_WIDTH_8;
-	info.mmc_dev_type = MMC_IS_EMMC;
-	imx_usdhc_init(&params, &info);
+	mmc_info.mmc_dev_type = MMC_IS_EMMC;
+	imx_usdhc_init(&params, &mmc_info);
 }
 
 static void picopi_setup_usb_clocks(void)
diff --git a/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c b/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c
index 935a411..ec13ade 100644
--- a/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c
+++ b/plat/imx/imx7/warp7/warp7_bl2_el3_setup.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2018-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -69,6 +69,8 @@
 	 IOMUXC_SW_PAD_CTL_PAD_ECSPI1_SCLK_HYS_EN		| \
 	 IOMUXC_SW_PAD_CTL_PAD_ECSPI1_SCLK_DSE_1_X4)
 
+static struct mmc_device_info mmc_info;
+
 static void warp7_setup_pinmux(void)
 {
 	/* Configure UART1 TX */
@@ -99,14 +101,13 @@
 static void warp7_usdhc_setup(void)
 {
 	imx_usdhc_params_t params;
-	struct mmc_device_info info;
 
 	zeromem(&params, sizeof(imx_usdhc_params_t));
 	params.reg_base = PLAT_WARP7_BOOT_MMC_BASE;
 	params.clk_rate = 25000000;
 	params.bus_width = MMC_BUS_WIDTH_8;
-	info.mmc_dev_type = MMC_IS_EMMC;
-	imx_usdhc_init(&params, &info);
+	mmc_info.mmc_dev_type = MMC_IS_EMMC;
+	imx_usdhc_init(&params, &mmc_info);
 }
 
 static void warp7_setup_usb_clocks(void)
diff --git a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c
index 307862d..2d67fdf 100644
--- a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c
+++ b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.c
@@ -143,6 +143,11 @@
 		blocked |= SPM_COND_CHECK_BLOCKED_PLL;
 	}
 
+	if (is_system_suspend && (blocked != 0U)) {
+		INFO("suspend: %s total blocked = 0x%08x\n",
+		     dest->name, blocked);
+	}
+
 	return blocked;
 }
 
diff --git a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h
index ba13fe3..91ebdd9 100644
--- a/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h
+++ b/plat/mediatek/mt8192/drivers/spm/mt_spm_cond.h
@@ -23,20 +23,11 @@
 	PLAT_SPM_COND_MAX,
 };
 
-enum PLAT_SPM_COND_PLL {
-	PLAT_SPM_COND_PLL_UNIVPLL = 0,
-	PLAT_SPM_COND_PLL_MFGPLL,
-	PLAT_SPM_COND_PLL_MSDCPLL,
-	PLAT_SPM_COND_PLL_TVDPLL,
-	PLAT_SPM_COND_PLL_MMPLL,
-	PLAT_SPM_COND_PLL_MAX,
-};
-
-#define PLL_BIT_MFGPLL	(PLAT_SPM_COND_PLL_MFGPLL)
-#define PLL_BIT_MMPLL	(PLAT_SPM_COND_PLL_MMPLL)
-#define PLL_BIT_UNIVPLL	(PLAT_SPM_COND_PLL_UNIVPLL)
-#define PLL_BIT_MSDCPLL	(PLAT_SPM_COND_PLL_MSDCPLL)
-#define PLL_BIT_TVDPLL	(PLAT_SPM_COND_PLL_TVDPLL)
+#define PLL_BIT_UNIVPLL	BIT(0)
+#define PLL_BIT_MFGPLL	BIT(1)
+#define PLL_BIT_MSDCPLL	BIT(2)
+#define PLL_BIT_TVDPLL	BIT(3)
+#define PLL_BIT_MMPLL	BIT(4)
 
 /* Definition about SPM_COND_CHECK_BLOCKED
  * bit [00 ~ 15]: cg blocking index
diff --git a/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c b/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c
index 4487348..b40fa87 100644
--- a/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c
+++ b/plat/mediatek/mt8195/drivers/spm/mt_spm_suspend.c
@@ -97,7 +97,7 @@
 	/* [4] */
 	.reg_spm_vrf18_req = 0,
 	/* [7] FIXME: default disable HW Auto S1*/
-	.reg_spm_ddr_en_req = 0,
+	.reg_spm_ddr_en_req = 1,
 	/* [8] */
 	.reg_spm_dvfs_req = 0,
 	/* [9] */
diff --git a/plat/rockchip/rk3399/drivers/dram/suspend.c b/plat/rockchip/rk3399/drivers/dram/suspend.c
index 7f9fad1..a8b1c32 100644
--- a/plat/rockchip/rk3399/drivers/dram/suspend.c
+++ b/plat/rockchip/rk3399/drivers/dram/suspend.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -49,6 +49,7 @@
 
 __pmusramdata uint32_t dpll_data[PLL_CON_COUNT];
 __pmusramdata uint32_t cru_clksel_con6;
+__pmusramdata uint8_t pmu_enable_watchdog0;
 
 /*
  * Copy @num registers from @src to @dst
@@ -562,8 +563,14 @@
 
 	/* LPDDR4 f2 cann't do training, all training will fail */
 	for (ch = 0; ch < ch_count; ch++) {
-		mmio_clrsetbits_32(PHY_REG(ch, 896), (0x3 << 8) | 1,
-				   fn << 8);
+		/*
+		 * Without this disabled for LPDDR4 we end up writing 0's
+		 * in place of real data in an interesting pattern.
+		 */
+		if (sdram_params->dramtype != LPDDR4) {
+			mmio_clrsetbits_32(PHY_REG(ch, 896), (0x3 << 8) | 1,
+					fn << 8);
+		}
 
 		/* data_training failed */
 		if (data_training(ch, sdram_params, PI_FULL_TRAINING))
@@ -748,13 +755,44 @@
 	phy_regs->phy896[0] &= ~(0x3 << 8);
 }
 
+__pmusramfunc void phy_dll_bypass_set(uint32_t ch, uint32_t freq)
+{
+	if (freq <= (125 * 1000 * 1000)) {
+		/* Set master mode to SW for slices*/
+		mmio_setbits_32(PHY_REG(ch, 86), 3 << 10);
+		mmio_setbits_32(PHY_REG(ch, 214), 3 << 10);
+		mmio_setbits_32(PHY_REG(ch, 342), 3 << 10);
+		mmio_setbits_32(PHY_REG(ch, 470), 3 << 10);
+		/* Set master mode to SW for address slices*/
+		mmio_setbits_32(PHY_REG(ch, 547), 3 << 18);
+		mmio_setbits_32(PHY_REG(ch, 675), 3 << 18);
+		mmio_setbits_32(PHY_REG(ch, 803), 3 << 18);
+	} else {
+		/* Clear SW master mode for slices*/
+		mmio_clrbits_32(PHY_REG(ch, 86), 3 << 10);
+		mmio_clrbits_32(PHY_REG(ch, 214), 3 << 10);
+		mmio_clrbits_32(PHY_REG(ch, 342), 3 << 10);
+		mmio_clrbits_32(PHY_REG(ch, 470), 3 << 10);
+		/* Clear SW master mode for address slices*/
+		mmio_clrbits_32(PHY_REG(ch, 547), 3 << 18);
+		mmio_clrbits_32(PHY_REG(ch, 675), 3 << 18);
+		mmio_clrbits_32(PHY_REG(ch, 803), 3 << 18);
+	}
+}
+
 __pmusramfunc void dmc_resume(void)
 {
 	struct rk3399_sdram_params *sdram_params = &sdram_config;
 	uint32_t channel_mask = 0;
 	uint32_t channel;
 
-	pmusram_enable_watchdog();
+	/*
+	 * We can't turn off the watchdog, so if we have not turned it on before
+	 * we should not turn it on here.
+	 */
+	if ((pmu_enable_watchdog0 & 0x1) == 0x1) {
+		pmusram_enable_watchdog();
+	}
 	pmu_sgrf_rst_hld_release();
 	restore_pmu_rsthold();
 	sram_secure_timer_init();
@@ -772,6 +810,13 @@
 retry:
 	for (channel = 0; channel < sdram_params->num_channels; channel++) {
 		phy_pctrl_reset(channel);
+		/*
+		 * Without this, LPDDR4 will write 0's in place of real data
+		 * in a strange pattern.
+		 */
+		if (sdram_params->dramtype == LPDDR4) {
+			phy_dll_bypass_set(channel, sdram_params->ddr_freq);
+		}
 		pctl_cfg(channel, sdram_params);
 	}
 
@@ -788,8 +833,12 @@
 		if (sdram_params->dramtype == LPDDR3)
 			sram_udelay(10);
 
-		/* If traning fail, retry to do it again. */
-		if (data_training(channel, sdram_params, PI_FULL_TRAINING))
+		/*
+		 * Training here will always fail for LPDDR4, so skip it
+		 * If traning fail, retry to do it again.
+		 */
+		if (sdram_params->dramtype != LPDDR4 &&
+		    data_training(channel, sdram_params, PI_FULL_TRAINING))
 			goto retry;
 
 		set_ddrconfig(sdram_params, channel,
diff --git a/plat/rockchip/rk3399/drivers/dram/suspend.h b/plat/rockchip/rk3399/drivers/dram/suspend.h
index b99a926..1389944 100644
--- a/plat/rockchip/rk3399/drivers/dram/suspend.h
+++ b/plat/rockchip/rk3399/drivers/dram/suspend.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -7,6 +7,7 @@
 #ifndef SUSPEND_H
 #define SUSPEND_H
 
+#include <stdint.h>
 #include <dram.h>
 
 #define KHz (1000)
@@ -22,5 +23,6 @@
 
 void dmc_suspend(void);
 __pmusramfunc void dmc_resume(void);
+extern __pmusramdata uint8_t pmu_enable_watchdog0;
 
 #endif /* SUSPEND_H */
diff --git a/plat/rockchip/rk3399/drivers/pmu/pmu.c b/plat/rockchip/rk3399/drivers/pmu/pmu.c
index faee678..3084c4f 100644
--- a/plat/rockchip/rk3399/drivers/pmu/pmu.c
+++ b/plat/rockchip/rk3399/drivers/pmu/pmu.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -1324,6 +1324,7 @@
 		store_wdt0[i] = mmio_read_32(WDT0_BASE + i * 4);
 		store_wdt1[i] = mmio_read_32(WDT1_BASE + i * 4);
 	}
+	pmu_enable_watchdog0 = (uint8_t) store_wdt0[0] & 0x1;
 }
 
 void wdt_register_restore(void)
diff --git a/plat/rpi/rpi4/platform.mk b/plat/rpi/rpi4/platform.mk
index 99d51fb..528eb1d 100644
--- a/plat/rpi/rpi4/platform.mk
+++ b/plat/rpi/rpi4/platform.mk
@@ -1,5 +1,5 @@
 #
-# Copyright (c) 2013-2019, ARM Limited and Contributors. All rights reserved.
+# Copyright (c) 2013-2021, ARM Limited and Contributors. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
@@ -86,6 +86,9 @@
 # Use normal memory mapping for ROM, FIP, SRAM and DRAM
 RPI3_USE_UEFI_MAP		:= 0
 
+# SMCCC PCI support (should be enabled for ACPI builds)
+SMC_PCI_SUPPORT            	:= 0
+
 # Process platform flags
 # ----------------------
 
@@ -96,6 +99,7 @@
 endif
 $(eval $(call add_define,RPI3_RUNTIME_UART))
 $(eval $(call add_define,RPI3_USE_UEFI_MAP))
+$(eval $(call add_define,SMC_PCI_SUPPORT))
 
 ifeq (${ARCH},aarch32)
   $(error Error: AArch32 not supported on rpi4)
@@ -105,3 +109,8 @@
 PLAT_BL_COMMON_SOURCES	+=	drivers/rpi3/rng/rpi3_rng.c		\
 				plat/rpi/common/rpi3_stack_protector.c
 endif
+
+ifeq ($(SMC_PCI_SUPPORT), 1)
+BL31_SOURCES            +=      plat/rpi/rpi4/rpi4_pci_svc.c
+endif
+
diff --git a/plat/rpi/rpi4/rpi4_pci_svc.c b/plat/rpi/rpi4/rpi4_pci_svc.c
new file mode 100644
index 0000000..7d1ca5c
--- /dev/null
+++ b/plat/rpi/rpi4/rpi4_pci_svc.c
@@ -0,0 +1,215 @@
+/*
+ * Copyright (c) 2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * The RPi4 has a single nonstandard PCI config region. It is broken into two
+ * pieces, the root port config registers and a window to a single device's
+ * config space which can move between devices. There isn't (yet) an
+ * authoritative public document on this since the available BCM2711 reference
+ * notes that there is a PCIe root port in the memory map but doesn't describe
+ * it. Given that it's not ECAM compliant yet reasonably simple, it makes for
+ * an excellent example of the PCI SMCCC interface.
+ *
+ * The PCI SMCCC interface is described in DEN0115 availabe from:
+ * https://developer.arm.com/documentation/den0115/latest
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <lib/pmf/pmf.h>
+#include <lib/runtime_instr.h>
+#include <services/pci_svc.h>
+#include <services/sdei.h>
+#include <services/std_svc.h>
+#include <smccc_helpers.h>
+
+#include <lib/mmio.h>
+
+static spinlock_t pci_lock;
+
+#define PCIE_REG_BASE		U(RPI_IO_BASE + 0x01500000)
+#define PCIE_MISC_PCIE_STATUS	0x4068
+#define PCIE_EXT_CFG_INDEX	0x9000
+/* A small window pointing at the ECAM of the device selected by CFG_INDEX */
+#define PCIE_EXT_CFG_DATA	0x8000
+#define INVALID_PCI_ADDR	0xFFFFFFFF
+
+#define	PCIE_EXT_BUS_SHIFT	20
+#define	PCIE_EXT_DEV_SHIFT	15
+#define	PCIE_EXT_FUN_SHIFT	12
+
+
+static uint64_t pci_segment_lib_get_base(uint32_t address, uint32_t offset)
+{
+	uint64_t	base;
+	uint32_t	bus, dev, fun;
+	uint32_t	status;
+
+	base = PCIE_REG_BASE;
+
+	offset &= PCI_OFFSET_MASK;  /* Pick off the 4k register offset */
+
+	/* The root port is at the base of the PCIe register space */
+	if (address != 0U) {
+		/*
+		 * The current device must be at CFG_DATA, a 4K window mapped,
+		 * via CFG_INDEX, to the device we are accessing. At the same
+		 * time we must avoid accesses to certain areas of the cfg
+		 * space via CFG_DATA. Detect those accesses and report that
+		 * the address is invalid.
+		 */
+		base += PCIE_EXT_CFG_DATA;
+		bus = PCI_ADDR_BUS(address);
+		dev = PCI_ADDR_DEV(address);
+		fun = PCI_ADDR_FUN(address);
+		address = (bus << PCIE_EXT_BUS_SHIFT) |
+			  (dev << PCIE_EXT_DEV_SHIFT) |
+			  (fun << PCIE_EXT_FUN_SHIFT);
+
+		/* Allow only dev = 0 on root port and bus 1 */
+		if ((bus < 2U) && (dev > 0U)) {
+			return INVALID_PCI_ADDR;
+		}
+
+		/* Assure link up before reading bus 1 */
+		status = mmio_read_32(PCIE_REG_BASE + PCIE_MISC_PCIE_STATUS);
+		if ((status & 0x30) != 0x30) {
+			return INVALID_PCI_ADDR;
+		}
+
+		/* Adjust which device the CFG_DATA window is pointing at */
+		mmio_write_32(PCIE_REG_BASE + PCIE_EXT_CFG_INDEX, address);
+	}
+	return base + offset;
+}
+
+/**
+ * pci_read_config() - Performs a config space read at addr
+ * @addr: 32-bit, segment, BDF of requested function encoded per DEN0115
+ * @off:  register offset of function described by @addr to read
+ * @sz:	  size of read (8,16,32) bits.
+ * @val:  returned zero extended value read from config space
+ *
+ * sz bits of PCI config space is read at addr:offset, and the value
+ * is returned in val. Invalid segment/offset values return failure.
+ * Reads to valid functions that don't exist return INVALID_PCI_ADDR
+ * as is specified by PCI for requests that aren't completed by EPs.
+ * The boilerplate in pci_svc.c tends to do basic segment, off
+ * and sz validation. This routine should avoid duplicating those
+ * checks.
+ *
+ * This function maps directly to the PCI_READ function in DEN0115
+ * where detailed requirements may be found.
+ *
+ * Return: SMC_PCI_CALL_SUCCESS with val set
+ *	   SMC_PCI_CALL_INVAL_PARAM, on parameter error
+ */
+uint32_t pci_read_config(uint32_t addr, uint32_t off, uint32_t sz, uint32_t *val)
+{
+	uint32_t ret = SMC_PCI_CALL_SUCCESS;
+	uint64_t base;
+
+	spin_lock(&pci_lock);
+	base = pci_segment_lib_get_base(addr, off);
+
+	if (base == INVALID_PCI_ADDR) {
+		*val = base;
+	} else {
+		switch (sz) {
+		case SMC_PCI_SZ_8BIT:
+			*val = mmio_read_8(base);
+			break;
+		case SMC_PCI_SZ_16BIT:
+			*val = mmio_read_16(base);
+			break;
+		case SMC_PCI_SZ_32BIT:
+			*val = mmio_read_32(base);
+			break;
+		default: /* should be unreachable */
+			*val = 0;
+			ret = SMC_PCI_CALL_INVAL_PARAM;
+		}
+	}
+	spin_unlock(&pci_lock);
+	return ret;
+}
+
+/**
+ * pci_write_config() - Performs a config space write at addr
+ * @addr: 32-bit, segment, BDF of requested function encoded per DEN0115
+ * @off:  register offset of function described by @addr to write
+ * @sz:	  size of write (8,16,32) bits.
+ * @val:  value to be written
+ *
+ * sz bits of PCI config space is written at addr:offset. Invalid
+ * segment/BDF values return failure. Writes to valid functions
+ * without valid EPs are ignored, as is specified by PCI.
+ * The boilerplate in pci_svc.c tends to do basic segment, off
+ * and sz validation, so it shouldn't need to be repeated here.
+ *
+ * This function maps directly to the PCI_WRITE function in DEN0115
+ * where detailed requirements may be found.
+ *
+ * Return: SMC_PCI_CALL_SUCCESS
+ *	   SMC_PCI_CALL_INVAL_PARAM, on parameter error
+ */
+uint32_t pci_write_config(uint32_t addr, uint32_t off, uint32_t sz, uint32_t val)
+{
+	uint32_t ret = SMC_PCI_CALL_SUCCESS;
+	uint64_t base;
+
+	spin_lock(&pci_lock);
+	base = pci_segment_lib_get_base(addr, off);
+
+	if (base != INVALID_PCI_ADDR) {
+		switch (sz) {
+		case SMC_PCI_SZ_8BIT:
+			mmio_write_8(base, val);
+			break;
+		case SMC_PCI_SZ_16BIT:
+			mmio_write_16(base, val);
+			break;
+		case SMC_PCI_SZ_32BIT:
+			mmio_write_32(base, val);
+			break;
+		default: /* should be unreachable */
+			ret = SMC_PCI_CALL_INVAL_PARAM;
+		}
+	}
+	spin_unlock(&pci_lock);
+	return ret;
+}
+
+/**
+ * pci_get_bus_for_seg() - returns the start->end bus range for a segment
+ * @seg:  segment being queried
+ * @bus_range:	returned bus begin + (end << 8)
+ * @nseg: returns next segment in this machine or 0 for end
+ *
+ * pci_get_bus_for_seg is called to check if a given segment is
+ * valid on this machine. If it is valid, then its bus ranges are
+ * returned along with the next valid segment on the machine. If
+ * this is the last segment, then nseg must be 0.
+ *
+ * This function maps directly to the PCI_GET_SEG_INFO function
+ * in DEN0115 where detailed requirements may be found.
+ *
+ * Return: SMC_PCI_CALL_SUCCESS, and appropriate bus_range and nseg
+ *	   SMC_PCI_CALL_NOT_IMPL, if the segment is invalid
+ */
+uint32_t pci_get_bus_for_seg(uint32_t seg, uint32_t *bus_range, uint32_t *nseg)
+{
+	uint32_t ret = SMC_PCI_CALL_SUCCESS;
+	*nseg = 0U; /* only a single segment */
+	if (seg == 0U) {
+		*bus_range = 0xFF00; /* start 0, end 255 */
+	} else {
+		*bus_range = 0U;
+		ret = SMC_PCI_CALL_NOT_IMPL;
+	}
+	return ret;
+}
diff --git a/services/std_svc/sdei/sdei_intr_mgmt.c b/services/std_svc/sdei/sdei_intr_mgmt.c
index fa1d3d2..5d176c2 100644
--- a/services/std_svc/sdei/sdei_intr_mgmt.c
+++ b/services/std_svc/sdei/sdei_intr_mgmt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ * Copyright (c) 2017-2021, ARM Limited and Contributors. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -8,6 +8,7 @@
 #include <string.h>
 
 #include <arch_helpers.h>
+#include <arch_features.h>
 #include <bl31/ehf.h>
 #include <bl31/interrupt_mgmt.h>
 #include <common/bl_common.h>
@@ -232,6 +233,77 @@
 }
 
 /*
+ * Prepare for ERET:
+ * - Set the ELR to the registered handler address
+ * - Set the SPSR register as described in the SDEI documentation and
+ *   the AArch64.TakeException() pseudocode function in
+ *   ARM DDI 0487F.c page J1-7635
+ */
+
+static void sdei_set_elr_spsr(sdei_entry_t *se, sdei_dispatch_context_t *disp_ctx)
+{
+	unsigned int client_el = sdei_client_el();
+	u_register_t sdei_spsr = SPSR_64(client_el, MODE_SP_ELX,
+					DISABLE_ALL_EXCEPTIONS);
+
+	u_register_t interrupted_pstate = disp_ctx->spsr_el3;
+
+	/* Check the SPAN bit in the client el SCTLR */
+	u_register_t client_el_sctlr;
+
+	if (client_el == MODE_EL2) {
+		client_el_sctlr = read_sctlr_el2();
+	} else {
+		client_el_sctlr = read_sctlr_el1();
+	}
+
+	/*
+	 * Check whether to force the PAN bit or use the value in the
+	 * interrupted EL according to the check described in
+	 * TakeException. Since the client can only be Non-Secure
+	 * EL2 or El1 some of the conditions in ElIsInHost() we know
+	 * will always be True.
+	 * When the client_el is EL2 we know that there will be a SPAN
+	 * bit in SCTLR_EL2 as we have already checked for the condition
+	 * HCR_EL2.E2H = 1 and HCR_EL2.TGE = 1
+	 */
+	u_register_t hcr_el2 = read_hcr();
+	bool el_is_in_host = is_armv8_1_vhe_present() &&
+			     (hcr_el2 & HCR_TGE_BIT) &&
+			     (hcr_el2 & HCR_E2H_BIT);
+
+	if (is_armv8_1_pan_present() &&
+	    ((client_el == MODE_EL1) ||
+		(client_el == MODE_EL2 && el_is_in_host)) &&
+	    ((client_el_sctlr & SCTLR_SPAN_BIT) == 0U)) {
+		sdei_spsr |=  SPSR_PAN_BIT;
+	} else {
+		sdei_spsr |= (interrupted_pstate & SPSR_PAN_BIT);
+	}
+
+	/* If SSBS is implemented, take the value from the client el SCTLR */
+	u_register_t ssbs_enabled = (read_id_aa64pfr1_el1()
+					>> ID_AA64PFR1_EL1_SSBS_SHIFT)
+					& ID_AA64PFR1_EL1_SSBS_MASK;
+	if (ssbs_enabled != SSBS_UNAVAILABLE) {
+		u_register_t  ssbs_bit = ((client_el_sctlr & SCTLR_DSSBS_BIT)
+						>> SCTLR_DSSBS_SHIFT)
+						<< SPSR_SSBS_SHIFT_AARCH64;
+		sdei_spsr |= ssbs_bit;
+	}
+
+	/* If MTE is implemented in the client el set the TCO bit */
+	if (get_armv8_5_mte_support() >= MTE_IMPLEMENTED_ELX) {
+		sdei_spsr |= SPSR_TCO_BIT_AARCH64;
+	}
+
+	/* Take the DIT field from the pstate of the interrupted el */
+	sdei_spsr |= (interrupted_pstate & SPSR_DIT_BIT);
+
+	cm_set_elr_spsr_el3(NON_SECURE, (uintptr_t) se->ep, sdei_spsr);
+}
+
+/*
  * Populate the Non-secure context so that the next ERET will dispatch to the
  * SDEI client.
  */
@@ -256,15 +328,8 @@
 	SMC_SET_GP(ctx, CTX_GPREG_X2, disp_ctx->elr_el3);
 	SMC_SET_GP(ctx, CTX_GPREG_X3, disp_ctx->spsr_el3);
 
-	/*
-	 * Prepare for ERET:
-	 *
-	 * - Set PC to the registered handler address
-	 * - Set SPSR to jump to client EL with exceptions masked
-	 */
-	cm_set_elr_spsr_el3(NON_SECURE, (uintptr_t) se->ep,
-			SPSR_64(sdei_client_el(), MODE_SP_ELX,
-				DISABLE_ALL_EXCEPTIONS));
+	/* Setup the elr and spsr register to prepare for ERET */
+	sdei_set_elr_spsr(se, disp_ctx);
 
 #if DYNAMIC_WORKAROUND_CVE_2018_3639
 	cve_2018_3639_t *tgt_cve_2018_3639;
@@ -571,15 +636,15 @@
 	if (!can_sdei_state_trans(se, DO_DISPATCH))
 		return -1;
 
-	/* Activate the priority corresponding to the event being dispatched */
-	ehf_activate_priority(sdei_event_priority(map));
-
 	/*
 	 * Prepare for NS dispatch by restoring the Non-secure context and
 	 * marking that as active.
 	 */
 	ns_ctx = restore_and_resume_ns_context();
 
+	/* Activate the priority corresponding to the event being dispatched */
+	ehf_activate_priority(sdei_event_priority(map));
+
 	/* Dispatch event synchronously */
 	setup_ns_dispatch(map, se, ns_ctx, &dispatch_jmp);
 	begin_sdei_synchronous_dispatch(&dispatch_jmp);