Adding upstream version 2.8.0+dfsg.upstream/2.8.0+dfsgupstream

Signed-off-by: Daniel Baumann <daniel.baumann@progress-linux.org>

67 files changed, 13878 insertions, 0 deletions

diff --git a/drivers/arm/cci/cci.c b/drivers/arm/cci/cci.c
new file mode 100644
index 0000000..2adfe17
--- /dev/null
+++ b/drivers/arm/cci/cci.c
@@ -0,0 +1,186 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/cci.h>
+#include <lib/mmio.h>
+
+#define MAKE_CCI_PART_NUMBER(hi, lo)	(((hi) << 8) | (lo))
+#define CCI_PART_LO_MASK		U(0xff)
+#define CCI_PART_HI_MASK		U(0xf)
+
+/* CCI part number codes read from Peripheral ID registers 0 and 1 */
+#define CCI400_PART_NUM		0x420
+#define CCI500_PART_NUM		0x422
+#define CCI550_PART_NUM		0x423
+
+#define CCI400_SLAVE_PORTS	5
+#define CCI500_SLAVE_PORTS	7
+#define CCI550_SLAVE_PORTS	7
+
+static uintptr_t cci_base;
+static const int *cci_slave_if_map;
+
+#if ENABLE_ASSERTIONS
+static unsigned int max_master_id;
+static int cci_num_slave_ports;
+
+static bool validate_cci_map(const int *map)
+{
+	unsigned int valid_cci_map = 0U;
+	int slave_if_id;
+	unsigned int i;
+
+	/* Validate the map */
+	for (i = 0U; i <= max_master_id; i++) {
+		slave_if_id = map[i];
+
+		if (slave_if_id < 0)
+			continue;
+
+		if (slave_if_id >= cci_num_slave_ports) {
+			ERROR("Slave interface ID is invalid\n");
+			return false;
+		}
+
+		if ((valid_cci_map & (1UL << slave_if_id)) != 0U) {
+			ERROR("Multiple masters are assigned same slave interface ID\n");
+			return false;
+		}
+		valid_cci_map |= 1UL << slave_if_id;
+	}
+
+	if (valid_cci_map == 0U) {
+		ERROR("No master is assigned a valid slave interface\n");
+		return false;
+	}
+
+	return true;
+}
+
+/*
+ * Read CCI part number from Peripheral ID registers
+ */
+static unsigned int read_cci_part_number(uintptr_t base)
+{
+	unsigned int part_lo, part_hi;
+
+	part_lo = mmio_read_32(base + PERIPHERAL_ID0) & CCI_PART_LO_MASK;
+	part_hi = mmio_read_32(base + PERIPHERAL_ID1) & CCI_PART_HI_MASK;
+
+	return MAKE_CCI_PART_NUMBER(part_hi, part_lo);
+}
+
+/*
+ * Identify a CCI device, and return the number of slaves. Return -1 for an
+ * unidentified device.
+ */
+static int get_slave_ports(unsigned int part_num)
+{
+	int num_slave_ports = -1;
+
+	switch (part_num) {
+
+	case CCI400_PART_NUM:
+		num_slave_ports = CCI400_SLAVE_PORTS;
+		break;
+	case CCI500_PART_NUM:
+		num_slave_ports = CCI500_SLAVE_PORTS;
+		break;
+	case CCI550_PART_NUM:
+		num_slave_ports = CCI550_SLAVE_PORTS;
+		break;
+	default:
+		/* Do nothing in default case */
+		break;
+	}
+
+	return num_slave_ports;
+}
+#endif /* ENABLE_ASSERTIONS */
+
+void __init cci_init(uintptr_t base, const int *map,
+				unsigned int num_cci_masters)
+{
+	assert(map != NULL);
+	assert(base != 0U);
+
+	cci_base = base;
+	cci_slave_if_map = map;
+
+#if ENABLE_ASSERTIONS
+	/*
+	 * Master Id's are assigned from zero, So in an array of size n
+	 * the max master id is (n - 1).
+	 */
+	max_master_id = num_cci_masters - 1U;
+	cci_num_slave_ports = get_slave_ports(read_cci_part_number(base));
+#endif
+	assert(cci_num_slave_ports >= 0);
+
+	assert(validate_cci_map(map));
+}
+
+void cci_enable_snoop_dvm_reqs(unsigned int master_id)
+{
+	int slave_if_id = cci_slave_if_map[master_id];
+
+	assert(master_id <= max_master_id);
+	assert((slave_if_id < cci_num_slave_ports) && (slave_if_id >= 0));
+	assert(cci_base != 0U);
+
+	/*
+	 * Enable Snoops and DVM messages, no need for Read/Modify/Write as
+	 * rest of bits are write ignore
+	 */
+	mmio_write_32(cci_base +
+		      SLAVE_IFACE_OFFSET(slave_if_id) + SNOOP_CTRL_REG,
+		      DVM_EN_BIT | SNOOP_EN_BIT);
+
+	/*
+	 * Wait for the completion of the write to the Snoop Control Register
+	 * before testing the change_pending bit
+	 */
+	dsbish();
+
+	/* Wait for the dust to settle down */
+	while ((mmio_read_32(cci_base + STATUS_REG) & CHANGE_PENDING_BIT) != 0U)
+		;
+}
+
+void cci_disable_snoop_dvm_reqs(unsigned int master_id)
+{
+	int slave_if_id = cci_slave_if_map[master_id];
+
+	assert(master_id <= max_master_id);
+	assert((slave_if_id < cci_num_slave_ports) && (slave_if_id >= 0));
+	assert(cci_base != 0U);
+
+	/*
+	 * Disable Snoops and DVM messages, no need for Read/Modify/Write as
+	 * rest of bits are write ignore.
+	 */
+	mmio_write_32(cci_base +
+		      SLAVE_IFACE_OFFSET(slave_if_id) + SNOOP_CTRL_REG,
+		      ~(DVM_EN_BIT | SNOOP_EN_BIT));
+
+	/*
+	 * Wait for the completion of the write to the Snoop Control Register
+	 * before testing the change_pending bit
+	 */
+	dsbish();
+
+	/* Wait for the dust to settle down */
+	while ((mmio_read_32(cci_base + STATUS_REG) & CHANGE_PENDING_BIT) != 0U)
+		;
+}
+
diff --git a/drivers/arm/ccn/ccn.c b/drivers/arm/ccn/ccn.c
new file mode 100644
index 0000000..5b13250
--- /dev/null
+++ b/drivers/arm/ccn/ccn.c
@@ -0,0 +1,621 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+
+#include <arch.h>
+#include <common/debug.h>
+#include <drivers/arm/ccn.h>
+#include <lib/bakery_lock.h>
+#include <lib/mmio.h>
+
+#include "ccn_private.h"
+
+static const ccn_desc_t *ccn_plat_desc;
+#if defined(IMAGE_BL31) || (!defined(__aarch64__) && defined(IMAGE_BL32))
+DEFINE_BAKERY_LOCK(ccn_lock);
+#endif
+
+/*******************************************************************************
+ * This function takes the base address of the CCN's programmer's view (PV), a
+ * region ID of one of the 256 regions (0-255) and a register offset within the
+ * region. It converts the first two parameters into a base address and uses it
+ * to read the register at the offset.
+ ******************************************************************************/
+static inline unsigned long long ccn_reg_read(uintptr_t periphbase,
+			     unsigned int region_id,
+			     unsigned int register_offset)
+{
+	uintptr_t region_base;
+
+	assert(periphbase);
+	assert(region_id < REGION_ID_LIMIT);
+
+	region_base = periphbase + region_id_to_base(region_id);
+	return mmio_read_64(region_base + register_offset);
+}
+
+/*******************************************************************************
+ * This function takes the base address of the CCN's programmer's view (PV), a
+ * region ID of one of the 256 regions (0-255), a register offset within the
+ * region and a value. It converts the first two parameters into a base address
+ * and uses it to write the value in the register at the offset.
+ ******************************************************************************/
+static inline void ccn_reg_write(uintptr_t periphbase,
+			  unsigned int region_id,
+			  unsigned int register_offset,
+			  unsigned long long value)
+{
+	uintptr_t region_base;
+
+	assert(periphbase);
+	assert(region_id < REGION_ID_LIMIT);
+
+	region_base = periphbase + region_id_to_base(region_id);
+	mmio_write_64(region_base + register_offset, value);
+}
+
+#if ENABLE_ASSERTIONS
+
+typedef struct rn_info {
+		unsigned char node_desc[MAX_RN_NODES];
+	} rn_info_t;
+
+/*******************************************************************************
+ * This function takes the base address of the CCN's programmer's view (PV) and
+ * the node ID of a Request Node (RN-D or RN-I). It returns the maximum number
+ * of master interfaces resident on that node. This number is equal to the least
+ * significant two bits of the node type ID + 1.
+ ******************************************************************************/
+static unsigned int ccn_get_rni_mcount(uintptr_t periphbase,
+				       unsigned int rn_id)
+{
+	unsigned int rn_type_id;
+
+	/* Use the node id to find the type of RN-I/D node */
+	rn_type_id = get_node_type(ccn_reg_read(periphbase,
+						rn_id + RNI_REGION_ID_START,
+						REGION_ID_OFFSET));
+
+	/* Return the number master interfaces based on node type */
+	return rn_type_id_to_master_cnt(rn_type_id);
+}
+
+/*******************************************************************************
+ * This function reads the CCN registers to find the following information about
+ * the ACE/ACELite/ACELite+DVM/CHI interfaces resident on the various types of
+ * Request Nodes (RN-Fs, RN-Is and RN-Ds) in the system:
+ *
+ * 1. The total number of such interfaces that this CCN IP supports. This is the
+ *    cumulative number of interfaces across all Request node types. It is
+ *    passed back as the return value of this function.
+ *
+ * 2. The maximum number of interfaces of a type resident on a Request node of
+ *    one of the three types. This information is populated in the 'info'
+ *    array provided by the caller as described next.
+ *
+ *    The array has 64 entries. Each entry corresponds to a Request node. The
+ *    Miscellaneous node's programmer's view has RN-F, RN-I and RN-D ID
+ *    registers. For each RN-I and RN-D ID indicated as being present in these
+ *    registers, its identification register (offset 0xFF00) is read. This
+ *    register specifies the maximum number of master interfaces the node
+ *    supports. For RN-Fs it is assumed that there can be only a single fully
+ *    coherent master resident on each node. The counts for each type of node
+ *    are use to populate the array entry at the index corresponding to the node
+ *    ID i.e. rn_info[node ID] = <number of master interfaces>
+ ******************************************************************************/
+static unsigned int ccn_get_rn_master_info(uintptr_t periphbase,
+					   rn_info_t *info)
+{
+	unsigned int num_masters = 0;
+	rn_types_t rn_type;
+
+	assert (info);
+
+	for (rn_type = RN_TYPE_RNF; rn_type < NUM_RN_TYPES; rn_type++) {
+		unsigned int mn_reg_off, node_id;
+		unsigned long long rn_bitmap;
+
+		/*
+		 * RN-F, RN-I, RN-D node registers in the MN region occupy
+		 * contiguous 16 byte apart offsets.
+		 */
+		mn_reg_off = MN_RNF_NODEID_OFFSET + (rn_type << 4);
+		rn_bitmap = ccn_reg_read(periphbase, MN_REGION_ID, mn_reg_off);
+
+		FOR_EACH_PRESENT_NODE_ID(node_id, rn_bitmap) {
+			unsigned int node_mcount;
+
+			/*
+			 * A RN-F does not have a node type since it does not
+			 * export a programmer's interface. It can only have a
+			 * single fully coherent master residing on it. If the
+			 * offset of the MN(Miscellaneous Node) register points
+			 * to a RN-I/D node then the master count is set to the
+			 * maximum number of master interfaces that can possibly
+			 * reside on the node.
+			 */
+			node_mcount = (mn_reg_off == MN_RNF_NODEID_OFFSET ? 1 :
+				       ccn_get_rni_mcount(periphbase, node_id));
+
+			/*
+			 * Use this value to increment the maximum possible
+			 * master interfaces in the system.
+			 */
+			num_masters += node_mcount;
+
+			/*
+			 * Update the entry in 'info' for this node ID with
+			 * the maximum number of masters than can sit on
+			 * it. This information will be used to validate the
+			 * node information passed by the platform later.
+			 */
+			info->node_desc[node_id] = node_mcount;
+		}
+	}
+
+	return num_masters;
+}
+
+/*******************************************************************************
+ * This function validates parameters passed by the platform (in a debug build).
+ * It collects information about the maximum number of master interfaces that:
+ * a) the CCN IP can accommodate and
+ * b) can exist on each Request node.
+ * It compares this with the information provided by the platform to determine
+ * the validity of the latter.
+ ******************************************************************************/
+static void __init ccn_validate_plat_params(const ccn_desc_t *plat_desc)
+{
+	unsigned int master_id, num_rn_masters;
+	rn_info_t info = { {0} };
+
+	assert(plat_desc);
+	assert(plat_desc->periphbase);
+	assert(plat_desc->master_to_rn_id_map);
+	assert(plat_desc->num_masters);
+	assert(plat_desc->num_masters < CCN_MAX_RN_MASTERS);
+
+	/*
+	 * Find the number and properties of fully coherent, IO coherent and IO
+	 * coherent + DVM master interfaces
+	 */
+	num_rn_masters = ccn_get_rn_master_info(plat_desc->periphbase, &info);
+	assert(plat_desc->num_masters < num_rn_masters);
+
+	/*
+	 * Iterate through the Request nodes specified by the platform.
+	 * Decrement the count of the masters in the 'info' array for each
+	 * Request node encountered. If the count would drop below 0 then the
+	 * platform's view of this aspect of CCN configuration is incorrect.
+	 */
+	for (master_id = 0; master_id < plat_desc->num_masters; master_id++) {
+		unsigned int node_id;
+
+		node_id = plat_desc->master_to_rn_id_map[master_id];
+		assert(node_id < MAX_RN_NODES);
+		assert(info.node_desc[node_id]);
+		info.node_desc[node_id]--;
+	}
+}
+#endif /* ENABLE_ASSERTIONS */
+
+/*******************************************************************************
+ * This function validates parameters passed by the platform (in a debug build)
+ * and initialises its internal data structures. A lock is required to prevent
+ * simultaneous CCN operations at runtime (only BL31) to add and remove Request
+ * nodes from coherency.
+ ******************************************************************************/
+void __init ccn_init(const ccn_desc_t *plat_desc)
+{
+#if ENABLE_ASSERTIONS
+	ccn_validate_plat_params(plat_desc);
+#endif
+
+	ccn_plat_desc = plat_desc;
+}
+
+/*******************************************************************************
+ * This function converts a bit map of master interface IDs to a bit map of the
+ * Request node IDs that they reside on.
+ ******************************************************************************/
+static unsigned long long ccn_master_to_rn_id_map(unsigned long long master_map)
+{
+	unsigned long long rn_id_map = 0;
+	unsigned int node_id, iface_id;
+
+	assert(master_map);
+	assert(ccn_plat_desc);
+
+	FOR_EACH_PRESENT_MASTER_INTERFACE(iface_id, master_map) {
+		assert(iface_id < ccn_plat_desc->num_masters);
+
+		/* Convert the master ID into the node ID */
+		node_id = ccn_plat_desc->master_to_rn_id_map[iface_id];
+
+		/* Set the bit corresponding to this node ID */
+		rn_id_map |= (1ULL << node_id);
+	}
+
+	return rn_id_map;
+}
+
+/*******************************************************************************
+ * This function executes the necessary operations to add or remove Request node
+ * IDs specified in the 'rn_id_map' bitmap from the snoop/DVM domains specified
+ * in the 'hn_id_map'. The 'region_id' specifies the ID of the first HN-F/MN
+ * on which the operation should be performed. 'op_reg_offset' specifies the
+ * type of operation (add/remove). 'stat_reg_offset' specifies the register
+ * which should be polled to determine if the operation has completed or not.
+ ******************************************************************************/
+static void ccn_snoop_dvm_do_op(unsigned long long rn_id_map,
+				unsigned long long hn_id_map,
+				unsigned int region_id,
+				unsigned int op_reg_offset,
+				unsigned int stat_reg_offset)
+{
+	unsigned int start_region_id;
+
+	assert(ccn_plat_desc);
+	assert(ccn_plat_desc->periphbase);
+
+#if defined(IMAGE_BL31) || (!defined(__aarch64__) && defined(IMAGE_BL32))
+	bakery_lock_get(&ccn_lock);
+#endif
+	start_region_id = region_id;
+	FOR_EACH_PRESENT_REGION_ID(start_region_id, hn_id_map) {
+		ccn_reg_write(ccn_plat_desc->periphbase,
+			      start_region_id,
+			      op_reg_offset,
+			      rn_id_map);
+	}
+
+	start_region_id = region_id;
+
+	FOR_EACH_PRESENT_REGION_ID(start_region_id, hn_id_map) {
+		WAIT_FOR_DOMAIN_CTRL_OP_COMPLETION(start_region_id,
+						   stat_reg_offset,
+						   op_reg_offset,
+						   rn_id_map);
+	}
+
+#if defined(IMAGE_BL31) || (!defined(__aarch64__) && defined(IMAGE_BL32))
+	bakery_lock_release(&ccn_lock);
+#endif
+}
+
+/*******************************************************************************
+ * The following functions provide the boot and runtime API to the platform for
+ * adding and removing master interfaces from the snoop/DVM domains. A bitmap of
+ * master interfaces IDs is passed as a parameter. It is converted into a bitmap
+ * of Request node IDs using the mapping provided by the platform while
+ * initialising the driver.
+ * For example, consider a dual cluster system where the clusters have values 0
+ * & 1 in the affinity level 1 field of their respective MPIDRs. While
+ * initialising this driver, the platform provides the mapping between each
+ * cluster and the corresponding Request node. To add or remove a cluster from
+ * the snoop and dvm domain, the bit position corresponding to the cluster ID
+ * should be set in the 'master_iface_map' i.e. to remove both clusters the
+ * bitmap would equal 0x11.
+ ******************************************************************************/
+void ccn_enter_snoop_dvm_domain(unsigned long long master_iface_map)
+{
+	unsigned long long rn_id_map;
+
+	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_HN_NODEID_MAP(ccn_plat_desc->periphbase,
+						  MN_HNF_NODEID_OFFSET),
+			    HNF_REGION_ID_START,
+			    HNF_SDC_SET_OFFSET,
+			    HNF_SDC_STAT_OFFSET);
+
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_SET_OFFSET,
+			    MN_DDC_STAT_OFFSET);
+}
+
+void ccn_exit_snoop_dvm_domain(unsigned long long master_iface_map)
+{
+	unsigned long long rn_id_map;
+
+	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_HN_NODEID_MAP(ccn_plat_desc->periphbase,
+						  MN_HNF_NODEID_OFFSET),
+			    HNF_REGION_ID_START,
+			    HNF_SDC_CLR_OFFSET,
+			    HNF_SDC_STAT_OFFSET);
+
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_CLR_OFFSET,
+			    MN_DDC_STAT_OFFSET);
+}
+
+void ccn_enter_dvm_domain(unsigned long long master_iface_map)
+{
+	unsigned long long rn_id_map;
+
+	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_SET_OFFSET,
+			    MN_DDC_STAT_OFFSET);
+}
+
+void ccn_exit_dvm_domain(unsigned long long master_iface_map)
+{
+	unsigned long long rn_id_map;
+
+	rn_id_map = ccn_master_to_rn_id_map(master_iface_map);
+	ccn_snoop_dvm_do_op(rn_id_map,
+			    CCN_GET_MN_NODEID_MAP(ccn_plat_desc->periphbase),
+			    MN_REGION_ID,
+			    MN_DDC_CLR_OFFSET,
+			    MN_DDC_STAT_OFFSET);
+}
+
+/*******************************************************************************
+ * This function returns the run mode of all the L3 cache partitions in the
+ * system. The state is expected to be one of NO_L3, SF_ONLY, L3_HAM or
+ * L3_FAM. Instead of comparing the states reported by all HN-Fs, the state of
+ * the first present HN-F node is reported. Since the driver does not export an
+ * interface to program them separately, there is no reason to perform this
+ * check. An HN-F could report that the L3 cache is transitioning from one mode
+ * to another e.g. HNF_PM_NOL3_2_SFONLY. In this case, the function waits for
+ * the transition to complete and reports the final state.
+ ******************************************************************************/
+unsigned int ccn_get_l3_run_mode(void)
+{
+	unsigned long long hnf_pstate_stat;
+
+	assert(ccn_plat_desc);
+	assert(ccn_plat_desc->periphbase);
+
+	/*
+	 * Wait for a L3 cache partition to enter any run mode. The pstate
+	 * parameter is read from an HN-F P-state status register. A non-zero
+	 * value in bits[1:0] means that the cache is transitioning to a run
+	 * mode.
+	 */
+	do {
+		hnf_pstate_stat = ccn_reg_read(ccn_plat_desc->periphbase,
+					       HNF_REGION_ID_START,
+					       HNF_PSTATE_STAT_OFFSET);
+	} while (hnf_pstate_stat & 0x3);
+
+	return PSTATE_TO_RUN_MODE(hnf_pstate_stat);
+}
+
+/*******************************************************************************
+ * This function sets the run mode of all the L3 cache partitions in the
+ * system to one of NO_L3, SF_ONLY, L3_HAM or L3_FAM depending upon the state
+ * specified by the 'mode' argument.
+ ******************************************************************************/
+void ccn_set_l3_run_mode(unsigned int mode)
+{
+	unsigned long long mn_hnf_id_map, hnf_pstate_stat;
+	unsigned int region_id;
+
+	assert(ccn_plat_desc);
+	assert(ccn_plat_desc->periphbase);
+	assert(mode <= CCN_L3_RUN_MODE_FAM);
+
+	mn_hnf_id_map = ccn_reg_read(ccn_plat_desc->periphbase,
+				     MN_REGION_ID,
+				     MN_HNF_NODEID_OFFSET);
+	region_id = HNF_REGION_ID_START;
+
+	/* Program the desired run mode */
+	FOR_EACH_PRESENT_REGION_ID(region_id, mn_hnf_id_map) {
+		ccn_reg_write(ccn_plat_desc->periphbase,
+			      region_id,
+			      HNF_PSTATE_REQ_OFFSET,
+			      mode);
+	}
+
+	/* Wait for the caches to transition to the run mode */
+	region_id = HNF_REGION_ID_START;
+	FOR_EACH_PRESENT_REGION_ID(region_id, mn_hnf_id_map) {
+		/*
+		 * Wait for a L3 cache partition to enter a target run
+		 * mode. The pstate parameter is read from an HN-F P-state
+		 * status register.
+		 */
+		do {
+			hnf_pstate_stat = ccn_reg_read(ccn_plat_desc->periphbase,
+					       region_id,
+					       HNF_PSTATE_STAT_OFFSET);
+		} while (((hnf_pstate_stat & HNF_PSTATE_MASK) >> 2) != mode);
+	}
+}
+
+/*******************************************************************************
+ * This function configures system address map and provides option to enable the
+ * 3SN striping mode of Slave node operation. The Slave node IDs and the Top
+ * Address bit1 and bit0 are provided as parameters to this function. This
+ * configuration is needed only if network contains a single SN-F or 3 SN-F and
+ * must be completed before the first request by the system to normal memory.
+ ******************************************************************************/
+void ccn_program_sys_addrmap(unsigned int sn0_id,
+		 unsigned int sn1_id,
+		 unsigned int sn2_id,
+		 unsigned int top_addr_bit0,
+		 unsigned int top_addr_bit1,
+		 unsigned char three_sn_en)
+{
+	unsigned long long mn_hnf_id_map, hnf_sam_ctrl_value;
+	unsigned int region_id;
+
+	assert(ccn_plat_desc);
+	assert(ccn_plat_desc->periphbase);
+
+	mn_hnf_id_map = ccn_reg_read(ccn_plat_desc->periphbase,
+				     MN_REGION_ID,
+				     MN_HNF_NODEID_OFFSET);
+	region_id = HNF_REGION_ID_START;
+	hnf_sam_ctrl_value = MAKE_HNF_SAM_CTRL_VALUE(sn0_id,
+						     sn1_id,
+						     sn2_id,
+						     top_addr_bit0,
+						     top_addr_bit1,
+						     three_sn_en);
+
+	FOR_EACH_PRESENT_REGION_ID(region_id, mn_hnf_id_map) {
+
+		/* Program the SAM control register */
+		ccn_reg_write(ccn_plat_desc->periphbase,
+			      region_id,
+			      HNF_SAM_CTRL_OFFSET,
+			      hnf_sam_ctrl_value);
+	}
+
+}
+
+/*******************************************************************************
+ * This function returns the part0 id from the peripheralID 0 register
+ * in CCN. This id can be used to distinguish the CCN variant present in the
+ * system.
+ ******************************************************************************/
+int ccn_get_part0_id(uintptr_t periphbase)
+{
+	assert(periphbase);
+	return (int)(mmio_read_64(periphbase
+			+ MN_PERIPH_ID_0_1_OFFSET) & 0xFF);
+}
+
+/*******************************************************************************
+ * This function returns the region id corresponding to a node_id of node_type.
+ ******************************************************************************/
+static unsigned int get_region_id_for_node(node_types_t node_type,
+						unsigned int node_id)
+{
+	unsigned int mn_reg_off, region_id;
+	unsigned long long node_bitmap;
+	unsigned int loc_node_id, node_pos_in_map = 0;
+
+	assert(node_type < NUM_NODE_TYPES);
+	assert(node_id < MAX_RN_NODES);
+
+	switch (node_type) {
+	case NODE_TYPE_RNI:
+		region_id = RNI_REGION_ID_START;
+		break;
+	case NODE_TYPE_HNF:
+		region_id = HNF_REGION_ID_START;
+		break;
+	case NODE_TYPE_HNI:
+		region_id = HNI_REGION_ID_START;
+		break;
+	case NODE_TYPE_SN:
+		region_id = SBSX_REGION_ID_START;
+		break;
+	default:
+		ERROR("Un-supported Node Type = %d.\n", node_type);
+		assert(false);
+		return REGION_ID_LIMIT;
+	}
+	/*
+	 * RN-I, HN-F, HN-I, SN node registers in the MN region
+	 * occupy contiguous 16 byte apart offsets.
+	 *
+	 * RN-F and RN-D node are not supported as
+	 * none of them exposes any memory map to
+	 * configure any of their offset registers.
+	 */
+
+	mn_reg_off = MN_RNF_NODEID_OFFSET + (node_type << 4);
+	node_bitmap = ccn_reg_read(ccn_plat_desc->periphbase,
+					MN_REGION_ID, mn_reg_off);
+
+	assert((node_bitmap & (1ULL << (node_id))) != 0U);
+
+
+	FOR_EACH_PRESENT_NODE_ID(loc_node_id, node_bitmap) {
+		INFO("Index = %u with loc_nod=%u and input nod=%u\n",
+					node_pos_in_map, loc_node_id, node_id);
+		if (loc_node_id == node_id)
+			break;
+		node_pos_in_map++;
+	}
+
+	if (node_pos_in_map == CCN_MAX_RN_MASTERS) {
+		ERROR("Node Id = %d, is not found.\n", node_id);
+		assert(false);
+		return REGION_ID_LIMIT;
+	}
+
+	/*
+	 * According to section 3.1.1 in CCN specification, region offset for
+	 * the RN-I components is calculated as (128 + NodeID of RN-I).
+	 */
+	if (node_type == NODE_TYPE_RNI)
+		region_id += node_id;
+	else
+		region_id += node_pos_in_map;
+
+	return region_id;
+}
+
+/*******************************************************************************
+ * This function sets the value 'val' to the register at register_offset from
+ * the base address pointed to by the region_id.
+ * where, region id is mapped to a node_id of node_type.
+ ******************************************************************************/
+void ccn_write_node_reg(node_types_t node_type, unsigned int node_id,
+			unsigned int reg_offset, unsigned long long val)
+{
+	unsigned int region_id = get_region_id_for_node(node_type, node_id);
+
+	if (reg_offset > REGION_ID_OFFSET) {
+		ERROR("Invalid Register offset 0x%x is provided.\n",
+								reg_offset);
+		assert(false);
+		return;
+	}
+
+	/* Setting the value of Auxiliary Control Register of the Node */
+	ccn_reg_write(ccn_plat_desc->periphbase, region_id, reg_offset, val);
+	VERBOSE("Value is successfully written at address 0x%lx.\n",
+			(ccn_plat_desc->periphbase
+			+ region_id_to_base(region_id))
+			+ reg_offset);
+}
+
+/*******************************************************************************
+ * This function read the value 'val' stored in the register at register_offset
+ * from the base address pointed to by the region_id.
+ * where, region id is mapped to a node_id of node_type.
+ ******************************************************************************/
+unsigned long long ccn_read_node_reg(node_types_t node_type,
+					unsigned int node_id,
+					unsigned int reg_offset)
+{
+	unsigned long long val;
+	unsigned int region_id = get_region_id_for_node(node_type, node_id);
+
+	if (reg_offset > REGION_ID_OFFSET) {
+		ERROR("Invalid Register offset 0x%x is provided.\n",
+								reg_offset);
+		assert(false);
+		return ULL(0);
+	}
+
+	/* Setting the value of Auxiliary Control Register of the Node */
+	val = ccn_reg_read(ccn_plat_desc->periphbase, region_id, reg_offset);
+	VERBOSE("Value is successfully read from address 0x%lx.\n",
+			(ccn_plat_desc->periphbase
+			+ region_id_to_base(region_id))
+			+ reg_offset);
+
+	return val;
+}
diff --git a/drivers/arm/ccn/ccn_private.h b/drivers/arm/ccn/ccn_private.h
new file mode 100644
index 0000000..8a936d9
--- /dev/null
+++ b/drivers/arm/ccn/ccn_private.h
@@ -0,0 +1,233 @@
+/*
+ * Copyright (c) 2015-2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef CCN_PRIVATE_H
+#define CCN_PRIVATE_H
+
+/*
+ * A CCN implementation can have a maximum of 64 Request nodes with node IDs
+ * from 0-63. These IDs are split across the three types of Request nodes
+ * i.e. RN-F, RN-D and RN-I.
+ */
+#define MAX_RN_NODES		64
+
+/* Enum used to loop through the 3 types of Request nodes */
+typedef enum rn_types {
+	RN_TYPE_RNF = 0,
+	RN_TYPE_RNI,
+	RN_TYPE_RND,
+	NUM_RN_TYPES
+} rn_types_t;
+
+/* Macro to convert a region id to its base address */
+#define region_id_to_base(id)	((id) << 16)
+
+/*
+ * Macro to calculate the number of master interfaces resident on a RN-I/RN-D.
+ * Value of first two bits of the RN-I/D node type + 1 == Maximum number of
+ * ACE-Lite or ACE-Lite+DVM interfaces supported on this node. E.g.
+ *
+ * 0x14 : RN-I with 1 ACE-Lite interface
+ * 0x15 : RN-I with 2 ACE-Lite interfaces
+ * 0x16 : RN-I with 3 ACE-Lite interfaces
+ */
+#define rn_type_id_to_master_cnt(id)	(((id) & 0x3) + 1)
+
+/*
+ * Constants used to identify a region in the programmer's view. These are
+ * common for all regions.
+ */
+#define REGION_ID_LIMIT		256
+#define REGION_ID_OFFSET	0xFF00
+
+#define REGION_NODE_ID_SHIFT	8
+#define REGION_NODE_ID_MASK	0x7f
+#define get_node_id(id_reg)	(((id_reg) >> REGION_NODE_ID_SHIFT) \
+				 & REGION_NODE_ID_MASK)
+
+#define REGION_NODE_TYPE_SHIFT	0
+#define REGION_NODE_TYPE_MASK	0x1f
+#define get_node_type(id_reg)	(((id_reg) >> REGION_NODE_TYPE_SHIFT) \
+				 & REGION_NODE_TYPE_MASK)
+
+/* Common offsets of registers to enter or exit a snoop/dvm domain */
+#define DOMAIN_CTRL_STAT_OFFSET	0x0200
+#define DOMAIN_CTRL_SET_OFFSET	0x0210
+#define DOMAIN_CTRL_CLR_OFFSET	0x0220
+
+/*
+ * Thess macros are used to determine if an operation to add or remove a Request
+ * node from the snoop/dvm domain has completed. 'rn_id_map' is a bit map of
+ * nodes. It was used to program the SET or CLEAR control register. The type of
+ * register is specified by 'op_reg_offset'. 'status_reg' is the bit map of
+ * nodes currently present in the snoop/dvm domain. 'rn_id_map' and 'status_reg'
+ * are logically ANDed and the result it stored back in the 'status_reg'. There
+ * are two outcomes of this operation:
+ *
+ * 1. If the DOMAIN_CTRL_SET_OFFSET register was programmed, then the set bits in
+ *    'rn_id_map' should appear in 'status_reg' when the operation completes. So
+ *    after the AND operation, at some point of time 'status_reg' should equal
+ *    'rn_id_map'.
+ *
+ * 2. If the DOMAIN_CTRL_CLR_OFFSET register was programmed, then the set bits in
+ *    'rn_id_map' should disappear in 'status_reg' when the operation
+ *    completes. So after the AND operation, at some point of time 'status_reg'
+ *    should equal 0.
+ */
+#define WAIT_FOR_DOMAIN_CTRL_OP_COMPLETION(region_id, stat_reg_offset,		\
+					   op_reg_offset, rn_id_map)		\
+	{									\
+		unsigned long long status_reg;						\
+		do {								\
+			status_reg = ccn_reg_read((ccn_plat_desc->periphbase),	\
+						  (region_id),			\
+						  (stat_reg_offset));		\
+			status_reg &= (rn_id_map);				\
+		} while ((op_reg_offset) == DOMAIN_CTRL_SET_OFFSET ?		\
+			 (rn_id_map) != status_reg : status_reg);		\
+	}
+
+/*
+ * Region ID of the Miscellaneous Node is always 0 as its located at the base of
+ * the programmer's view.
+ */
+#define MN_REGION_ID		0
+
+#define MN_REGION_ID_START	0
+#define DEBUG_REGION_ID_START	1
+#define HNI_REGION_ID_START	8
+#define SBSX_REGION_ID_START	16
+#define HNF_REGION_ID_START	32
+#define XP_REGION_ID_START	64
+#define RNI_REGION_ID_START	128
+
+/* Selected register offsets from the base of a HNF region */
+#define HNF_CFG_CTRL_OFFSET	0x0000
+#define HNF_SAM_CTRL_OFFSET	0x0008
+#define HNF_PSTATE_REQ_OFFSET	0x0010
+#define HNF_PSTATE_STAT_OFFSET	0x0018
+#define HNF_SDC_STAT_OFFSET	DOMAIN_CTRL_STAT_OFFSET
+#define HNF_SDC_SET_OFFSET	DOMAIN_CTRL_SET_OFFSET
+#define HNF_SDC_CLR_OFFSET	DOMAIN_CTRL_CLR_OFFSET
+#define HNF_AUX_CTRL_OFFSET	0x0500
+
+/* Selected register offsets from the base of a MN region */
+#define MN_SAR_OFFSET		0x0000
+#define MN_RNF_NODEID_OFFSET	0x0180
+#define MN_RNI_NODEID_OFFSET	0x0190
+#define MN_RND_NODEID_OFFSET	0x01A0
+#define MN_HNF_NODEID_OFFSET	0x01B0
+#define MN_HNI_NODEID_OFFSET	0x01C0
+#define MN_SN_NODEID_OFFSET	0x01D0
+#define MN_DDC_STAT_OFFSET	DOMAIN_CTRL_STAT_OFFSET
+#define MN_DDC_SET_OFFSET	DOMAIN_CTRL_SET_OFFSET
+#define MN_DDC_CLR_OFFSET	DOMAIN_CTRL_CLR_OFFSET
+#define MN_PERIPH_ID_0_1_OFFSET	0xFE0
+#define MN_ID_OFFSET		REGION_ID_OFFSET
+
+/* HNF System Address Map register bit masks and shifts */
+#define HNF_SAM_CTRL_SN_ID_MASK		0x7f
+#define HNF_SAM_CTRL_SN0_ID_SHIFT	0
+#define HNF_SAM_CTRL_SN1_ID_SHIFT	8
+#define HNF_SAM_CTRL_SN2_ID_SHIFT	16
+
+#define HNF_SAM_CTRL_TAB0_MASK		ULL(0x3f)
+#define HNF_SAM_CTRL_TAB0_SHIFT		48
+#define HNF_SAM_CTRL_TAB1_MASK		ULL(0x3f)
+#define HNF_SAM_CTRL_TAB1_SHIFT		56
+
+#define HNF_SAM_CTRL_3SN_ENB_SHIFT	32
+#define HNF_SAM_CTRL_3SN_ENB_MASK	ULL(0x01)
+
+/*
+ * Macro to create a value suitable for programming into a HNF SAM Control
+ * register for enabling 3SN striping.
+ */
+#define MAKE_HNF_SAM_CTRL_VALUE(sn0, sn1, sn2, tab0, tab1, three_sn_en)     \
+	((((sn0) & HNF_SAM_CTRL_SN_ID_MASK) << HNF_SAM_CTRL_SN0_ID_SHIFT) | \
+	 (((sn1) & HNF_SAM_CTRL_SN_ID_MASK) << HNF_SAM_CTRL_SN1_ID_SHIFT) | \
+	 (((sn2) & HNF_SAM_CTRL_SN_ID_MASK) << HNF_SAM_CTRL_SN2_ID_SHIFT) | \
+	 (((tab0) & HNF_SAM_CTRL_TAB0_MASK) << HNF_SAM_CTRL_TAB0_SHIFT)   | \
+	 (((tab1) & HNF_SAM_CTRL_TAB1_MASK) << HNF_SAM_CTRL_TAB1_SHIFT)   | \
+	 (((three_sn_en) & HNF_SAM_CTRL_3SN_ENB_MASK) << HNF_SAM_CTRL_3SN_ENB_SHIFT))
+
+/* Mask to read the power state value from an HN-F P-state register */
+#define HNF_PSTATE_MASK		0xf
+
+/* Macro to extract the run mode from a p-state value */
+#define PSTATE_TO_RUN_MODE(pstate)	(((pstate) & HNF_PSTATE_MASK) >> 2)
+
+/*
+ * Helper macro that iterates through a given bit map. In each iteration,
+ * it returns the position of the set bit.
+ * It can be used by other utility macros to iterates through all nodes
+ * or masters given a bit map of them.
+ */
+#define FOR_EACH_BIT(bit_pos, bit_map)			\
+	for (bit_pos = __builtin_ctzll(bit_map);	\
+	     bit_map;					\
+	     bit_map &= ~(1ULL << (bit_pos)),		\
+	     bit_pos = __builtin_ctzll(bit_map))
+
+/*
+ * Utility macro that iterates through a bit map of node IDs. In each
+ * iteration, it returns the ID of the next present node in the bit map. Node
+ * ID of a present node == Position of set bit == Number of zeroes trailing the
+ * bit.
+ */
+#define FOR_EACH_PRESENT_NODE_ID(node_id, bit_map)	\
+		FOR_EACH_BIT(node_id, bit_map)
+
+/*
+ * Helper function to return number of set bits in bitmap
+ */
+static inline unsigned int count_set_bits(unsigned long long bitmap)
+{
+	unsigned int count = 0;
+
+	for (; bitmap; bitmap &= bitmap - 1)
+		++count;
+
+	return count;
+}
+
+/*
+ * Utility macro that iterates through a bit map of node IDs. In each iteration,
+ * it returns the ID of the next present region corresponding to a node present
+ * in the bit map. Region ID of a present node is in between passed region id
+ * and region id + number of set bits in the bitmap i.e. the number of present
+ * nodes.
+ */
+#define FOR_EACH_PRESENT_REGION_ID(region_id, bit_map)				\
+	for (unsigned long long region_id_limit = count_set_bits(bit_map)	\
+							+ region_id;		\
+	    region_id < region_id_limit;					\
+	    region_id++)
+
+/*
+ * Same macro as FOR_EACH_PRESENT_NODE, but renamed to indicate it traverses
+ * through a bit map of master interfaces.
+ */
+#define FOR_EACH_PRESENT_MASTER_INTERFACE(iface_id, bit_map)	\
+			FOR_EACH_BIT(iface_id, bit_map)
+
+/*
+ * Macro that returns the node id bit map for the Miscellaneous Node
+ */
+#define CCN_GET_MN_NODEID_MAP(periphbase)				\
+	(1 << get_node_id(ccn_reg_read(periphbase, MN_REGION_ID,	\
+						REGION_ID_OFFSET)))
+
+/*
+ * This macro returns the bitmap of Home nodes on the basis of the
+ * 'mn_hn_id_reg_offset' parameter from the Miscellaneous node's (MN)
+ * programmer's view. The MN has a register which carries the bitmap of present
+ * Home nodes of each type i.e. HN-Fs, HN-Is & HN-Ds.
+ */
+#define CCN_GET_HN_NODEID_MAP(periphbase, mn_hn_id_reg_offset)		\
+	ccn_reg_read(periphbase, MN_REGION_ID, mn_hn_id_reg_offset)
+
+#endif /* CCN_PRIVATE_H */
diff --git a/drivers/arm/css/mhu/css_mhu.c b/drivers/arm/css/mhu/css_mhu.c
new file mode 100644
index 0000000..b7faf7e
--- /dev/null
+++ b/drivers/arm/css/mhu/css_mhu.c
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2014-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <drivers/arm/css/css_mhu.h>
+#include <lib/bakery_lock.h>
+#include <lib/mmio.h>
+#include <plat/arm/common/plat_arm.h>
+
+/* SCP MHU secure channel registers */
+#define SCP_INTR_S_STAT		0x200
+#define SCP_INTR_S_SET		0x208
+#define SCP_INTR_S_CLEAR	0x210
+
+/* CPU MHU secure channel registers */
+#define CPU_INTR_S_STAT		0x300
+#define CPU_INTR_S_SET		0x308
+#define CPU_INTR_S_CLEAR	0x310
+
+ARM_INSTANTIATE_LOCK;
+
+/* Weak definition may be overridden in specific CSS based platform */
+#pragma weak plat_arm_pwrc_setup
+
+
+/*
+ * Slot 31 is reserved because the MHU hardware uses this register bit to
+ * indicate a non-secure access attempt. The total number of available slots is
+ * therefore 31 [30:0].
+ */
+#define MHU_MAX_SLOT_ID		30
+
+void mhu_secure_message_start(unsigned int slot_id)
+{
+	assert(slot_id <= MHU_MAX_SLOT_ID);
+
+	arm_lock_get();
+
+	/* Make sure any previous command has finished */
+	while (mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) &
+							(1 << slot_id))
+		;
+}
+
+void mhu_secure_message_send(unsigned int slot_id)
+{
+	assert(slot_id <= MHU_MAX_SLOT_ID);
+	assert(!(mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) &
+							(1 << slot_id)));
+
+	/* Send command to SCP */
+	mmio_write_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_SET, 1 << slot_id);
+}
+
+uint32_t mhu_secure_message_wait(void)
+{
+	/* Wait for response from SCP */
+	uint32_t response;
+	while (!(response = mmio_read_32(PLAT_CSS_MHU_BASE + SCP_INTR_S_STAT)))
+		;
+
+	return response;
+}
+
+void mhu_secure_message_end(unsigned int slot_id)
+{
+	assert(slot_id <= MHU_MAX_SLOT_ID);
+
+	/*
+	 * Clear any response we got by writing one in the relevant slot bit to
+	 * the CLEAR register
+	 */
+	mmio_write_32(PLAT_CSS_MHU_BASE + SCP_INTR_S_CLEAR, 1 << slot_id);
+
+	arm_lock_release();
+}
+
+void __init mhu_secure_init(void)
+{
+	arm_lock_init();
+
+	/*
+	 * The STAT register resets to zero. Ensure it is in the expected state,
+	 * as a stale or garbage value would make us think it's a message we've
+	 * already sent.
+	 */
+	assert(mmio_read_32(PLAT_CSS_MHU_BASE + CPU_INTR_S_STAT) == 0);
+}
+
+void __init plat_arm_pwrc_setup(void)
+{
+	mhu_secure_init();
+}
diff --git a/drivers/arm/css/mhu/css_mhu_doorbell.c b/drivers/arm/css/mhu/css_mhu_doorbell.c
new file mode 100644
index 0000000..c51f3b1
--- /dev/null
+++ b/drivers/arm/css/mhu/css_mhu_doorbell.c
@@ -0,0 +1,40 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <platform_def.h>
+
+#include <arch_helpers.h>
+#include <drivers/arm/css/css_mhu_doorbell.h>
+#include <drivers/arm/css/scmi.h>
+
+void mhu_ring_doorbell(struct scmi_channel_plat_info *plat_info)
+{
+	MHU_RING_DOORBELL(plat_info->db_reg_addr,
+			plat_info->db_modify_mask,
+			plat_info->db_preserve_mask);
+	return;
+}
+
+void mhuv2_ring_doorbell(struct scmi_channel_plat_info *plat_info)
+{
+	uintptr_t mhuv2_base = plat_info->db_reg_addr & MHU_V2_FRAME_BASE_MASK;
+
+	/* wake receiver */
+	MHU_V2_ACCESS_REQUEST(mhuv2_base);
+
+	/* wait for receiver to acknowledge its ready */
+	while (MHU_V2_IS_ACCESS_READY(mhuv2_base) == 0)
+		;
+
+	MHU_RING_DOORBELL(plat_info->db_reg_addr,
+			plat_info->db_modify_mask,
+			plat_info->db_preserve_mask);
+
+	/* clear the access request for the receiver */
+	MHU_V2_CLEAR_REQUEST(mhuv2_base);
+
+	return;
+}
diff --git a/drivers/arm/css/scmi/scmi_ap_core_proto.c b/drivers/arm/css/scmi/scmi_ap_core_proto.c
new file mode 100644
index 0000000..5941b87
--- /dev/null
+++ b/drivers/arm/css/scmi/scmi_ap_core_proto.c
@@ -0,0 +1,81 @@
+/*
+ * Copyright (c) 2018-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/scmi.h>
+
+#include "scmi_private.h"
+
+/*
+ * API to set the SCMI AP core reset address and attributes
+ */
+int scmi_ap_core_set_reset_addr(void *p, uint64_t reset_addr, uint32_t attr)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(SCMI_AP_CORE_PROTO_ID,
+			SCMI_AP_CORE_RESET_ADDR_SET_MSG, token);
+	mbx_mem->len = SCMI_AP_CORE_RESET_ADDR_SET_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+	SCMI_PAYLOAD_ARG3(mbx_mem->payload, reset_addr & 0xffffffff,
+		reset_addr >> 32, attr);
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL1(mbx_mem->payload, ret);
+	assert(mbx_mem->len == SCMI_AP_CORE_RESET_ADDR_SET_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
+
+/*
+ * API to get the SCMI AP core reset address and attributes
+ */
+int scmi_ap_core_get_reset_addr(void *p, uint64_t *reset_addr, uint32_t *attr)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+	uint32_t lo_addr, hi_addr;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(SCMI_AP_CORE_PROTO_ID,
+			SCMI_AP_CORE_RESET_ADDR_GET_MSG, token);
+	mbx_mem->len = SCMI_AP_CORE_RESET_ADDR_GET_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL4(mbx_mem->payload, ret, lo_addr, hi_addr, *attr);
+	*reset_addr = lo_addr | (uint64_t)hi_addr << 32;
+	assert(mbx_mem->len == SCMI_AP_CORE_RESET_ADDR_GET_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
diff --git a/drivers/arm/css/scmi/scmi_common.c b/drivers/arm/css/scmi/scmi_common.c
new file mode 100644
index 0000000..ec749fb
--- /dev/null
+++ b/drivers/arm/css/scmi/scmi_common.c
@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/scmi.h>
+
+#include "scmi_private.h"
+
+#if HW_ASSISTED_COHERENCY
+#define scmi_lock_init(lock)
+#define scmi_lock_get(lock)		spin_lock(lock)
+#define scmi_lock_release(lock)		spin_unlock(lock)
+#else
+#define scmi_lock_init(lock)		bakery_lock_init(lock)
+#define scmi_lock_get(lock)		bakery_lock_get(lock)
+#define scmi_lock_release(lock)		bakery_lock_release(lock)
+#endif
+
+
+/*
+ * Private helper function to get exclusive access to SCMI channel.
+ */
+void scmi_get_channel(scmi_channel_t *ch)
+{
+	assert(ch->lock);
+	scmi_lock_get(ch->lock);
+
+	/* Make sure any previous command has finished */
+	assert(SCMI_IS_CHANNEL_FREE(
+			((mailbox_mem_t *)(ch->info->scmi_mbx_mem))->status));
+}
+
+/*
+ * Private helper function to transfer ownership of channel from AP to SCP.
+ */
+void scmi_send_sync_command(scmi_channel_t *ch)
+{
+	mailbox_mem_t *mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+
+	SCMI_MARK_CHANNEL_BUSY(mbx_mem->status);
+
+	/*
+	 * Ensure that any write to the SCMI payload area is seen by SCP before
+	 * we write to the doorbell register. If these 2 writes were reordered
+	 * by the CPU then SCP would read stale payload data
+	 */
+	dmbst();
+
+	ch->info->ring_doorbell(ch->info);
+	/*
+	 * Ensure that the write to the doorbell register is ordered prior to
+	 * checking whether the channel is free.
+	 */
+	dmbsy();
+
+	/* Wait for channel to be free */
+	while (!SCMI_IS_CHANNEL_FREE(mbx_mem->status))
+		;
+
+	/*
+	 * Ensure that any read to the SCMI payload area is done after reading
+	 * mailbox status. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data
+	 */
+	dmbld();
+}
+
+/*
+ * Private helper function to release exclusive access to SCMI channel.
+ */
+void scmi_put_channel(scmi_channel_t *ch)
+{
+	/* Make sure any previous command has finished */
+	assert(SCMI_IS_CHANNEL_FREE(
+			((mailbox_mem_t *)(ch->info->scmi_mbx_mem))->status));
+
+	assert(ch->lock);
+	scmi_lock_release(ch->lock);
+}
+
+/*
+ * API to query the SCMI protocol version.
+ */
+int scmi_proto_version(void *p, uint32_t proto_id, uint32_t *version)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(proto_id, SCMI_PROTO_VERSION_MSG,
+							token);
+	mbx_mem->len = SCMI_PROTO_VERSION_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL2(mbx_mem->payload, ret, *version);
+	assert(mbx_mem->len == SCMI_PROTO_VERSION_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
+
+/*
+ * API to query the protocol message attributes for a SCMI protocol.
+ */
+int scmi_proto_msg_attr(void *p, uint32_t proto_id,
+		uint32_t command_id, uint32_t *attr)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(proto_id,
+				SCMI_PROTO_MSG_ATTR_MSG, token);
+	mbx_mem->len = SCMI_PROTO_MSG_ATTR_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+	SCMI_PAYLOAD_ARG1(mbx_mem->payload, command_id);
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL2(mbx_mem->payload, ret, *attr);
+	assert(mbx_mem->len == SCMI_PROTO_MSG_ATTR_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
+
+/*
+ * SCMI Driver initialization API. Returns initialized channel on success
+ * or NULL on error. The return type is an opaque void pointer.
+ */
+void *scmi_init(scmi_channel_t *ch)
+{
+	uint32_t version;
+	int ret;
+
+	assert(ch && ch->info);
+	assert(ch->info->db_reg_addr);
+	assert(ch->info->db_modify_mask);
+	assert(ch->info->db_preserve_mask);
+	assert(ch->info->ring_doorbell != NULL);
+
+	assert(ch->lock);
+
+	scmi_lock_init(ch->lock);
+
+	ch->is_initialized = 1;
+
+	ret = scmi_proto_version(ch, SCMI_PWR_DMN_PROTO_ID, &version);
+	if (ret != SCMI_E_SUCCESS) {
+		WARN("SCMI power domain protocol version message failed\n");
+		goto error;
+	}
+
+	if (!is_scmi_version_compatible(SCMI_PWR_DMN_PROTO_VER, version)) {
+		WARN("SCMI power domain protocol version 0x%x incompatible with driver version 0x%x\n",
+			version, SCMI_PWR_DMN_PROTO_VER);
+		goto error;
+	}
+
+	VERBOSE("SCMI power domain protocol version 0x%x detected\n", version);
+
+	ret = scmi_proto_version(ch, SCMI_SYS_PWR_PROTO_ID, &version);
+	if ((ret != SCMI_E_SUCCESS)) {
+		WARN("SCMI system power protocol version message failed\n");
+		goto error;
+	}
+
+	if (!is_scmi_version_compatible(SCMI_SYS_PWR_PROTO_VER, version)) {
+		WARN("SCMI system power management protocol version 0x%x incompatible with driver version 0x%x\n",
+			version, SCMI_SYS_PWR_PROTO_VER);
+		goto error;
+	}
+
+	VERBOSE("SCMI system power management protocol version 0x%x detected\n",
+						version);
+
+	INFO("SCMI driver initialized\n");
+
+	return (void *)ch;
+
+error:
+	ch->is_initialized = 0;
+	return NULL;
+}
diff --git a/drivers/arm/css/scmi/scmi_private.h b/drivers/arm/css/scmi/scmi_private.h
new file mode 100644
index 0000000..a684ca5
--- /dev/null
+++ b/drivers/arm/css/scmi/scmi_private.h
@@ -0,0 +1,160 @@
+/*
+ * Copyright (c) 2017-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SCMI_PRIVATE_H
+#define SCMI_PRIVATE_H
+
+#include <lib/mmio.h>
+
+/*
+ * SCMI power domain management protocol message and response lengths. It is
+ * calculated as sum of length in bytes of the message header (4) and payload
+ * area (the number of bytes of parameters or return values in the payload).
+ */
+#define SCMI_PROTO_VERSION_MSG_LEN		4
+#define SCMI_PROTO_VERSION_RESP_LEN		12
+
+#define SCMI_PROTO_MSG_ATTR_MSG_LEN		8
+#define SCMI_PROTO_MSG_ATTR_RESP_LEN		12
+
+#define SCMI_AP_CORE_RESET_ADDR_SET_MSG_LEN	16
+#define SCMI_AP_CORE_RESET_ADDR_SET_RESP_LEN	8
+
+#define SCMI_AP_CORE_RESET_ADDR_GET_MSG_LEN	4
+#define SCMI_AP_CORE_RESET_ADDR_GET_RESP_LEN	20
+
+#define SCMI_PWR_STATE_SET_MSG_LEN		16
+#define SCMI_PWR_STATE_SET_RESP_LEN		8
+
+#define SCMI_PWR_STATE_GET_MSG_LEN		8
+#define SCMI_PWR_STATE_GET_RESP_LEN		12
+
+#define SCMI_SYS_PWR_STATE_SET_MSG_LEN		12
+#define SCMI_SYS_PWR_STATE_SET_RESP_LEN		8
+
+#define SCMI_SYS_PWR_STATE_GET_MSG_LEN		4
+#define SCMI_SYS_PWR_STATE_GET_RESP_LEN		12
+
+/* SCMI message header format bit field */
+#define SCMI_MSG_ID_SHIFT		0
+#define SCMI_MSG_ID_WIDTH		8
+#define SCMI_MSG_ID_MASK		((1 << SCMI_MSG_ID_WIDTH) - 1)
+
+#define SCMI_MSG_TYPE_SHIFT		8
+#define SCMI_MSG_TYPE_WIDTH		2
+#define SCMI_MSG_TYPE_MASK		((1 << SCMI_MSG_TYPE_WIDTH) - 1)
+
+#define SCMI_MSG_PROTO_ID_SHIFT		10
+#define SCMI_MSG_PROTO_ID_WIDTH		8
+#define SCMI_MSG_PROTO_ID_MASK		((1 << SCMI_MSG_PROTO_ID_WIDTH) - 1)
+
+#define SCMI_MSG_TOKEN_SHIFT		18
+#define SCMI_MSG_TOKEN_WIDTH		10
+#define SCMI_MSG_TOKEN_MASK		((1 << SCMI_MSG_TOKEN_WIDTH) - 1)
+
+
+/* SCMI mailbox flags */
+#define SCMI_FLAG_RESP_POLL	0
+#define SCMI_FLAG_RESP_INT	1
+
+/* SCMI power domain protocol `POWER_STATE_SET` message flags */
+#define SCMI_PWR_STATE_SET_FLAG_SYNC	0
+#define SCMI_PWR_STATE_SET_FLAG_ASYNC	1
+
+/*
+ * Helper macro to create an SCMI message header given protocol, message id
+ * and token.
+ */
+#define SCMI_MSG_CREATE(_protocol, _msg_id, _token)				\
+	((((_protocol) & SCMI_MSG_PROTO_ID_MASK) << SCMI_MSG_PROTO_ID_SHIFT) |	\
+	(((_msg_id) & SCMI_MSG_ID_MASK) << SCMI_MSG_ID_SHIFT) |			\
+	(((_token) & SCMI_MSG_TOKEN_MASK) << SCMI_MSG_TOKEN_SHIFT))
+
+/* Helper macro to get the token from a SCMI message header */
+#define SCMI_MSG_GET_TOKEN(_msg)				\
+	(((_msg) >> SCMI_MSG_TOKEN_SHIFT) & SCMI_MSG_TOKEN_MASK)
+
+/* SCMI Channel Status bit fields */
+#define SCMI_CH_STATUS_RES0_MASK	0xFFFFFFFE
+#define SCMI_CH_STATUS_FREE_SHIFT	0
+#define SCMI_CH_STATUS_FREE_WIDTH	1
+#define SCMI_CH_STATUS_FREE_MASK	((1 << SCMI_CH_STATUS_FREE_WIDTH) - 1)
+
+/* Helper macros to check and write the channel status */
+#define SCMI_IS_CHANNEL_FREE(status)					\
+	(!!(((status) >> SCMI_CH_STATUS_FREE_SHIFT) & SCMI_CH_STATUS_FREE_MASK))
+
+#define SCMI_MARK_CHANNEL_BUSY(status)	do {				\
+		assert(SCMI_IS_CHANNEL_FREE(status));			\
+		(status) &= ~(SCMI_CH_STATUS_FREE_MASK <<		\
+				SCMI_CH_STATUS_FREE_SHIFT);		\
+	} while (0)
+
+/* Helper macros to copy arguments to the mailbox payload */
+#define SCMI_PAYLOAD_ARG1(payld_arr, arg1)				\
+		mmio_write_32((uintptr_t)&payld_arr[0], arg1)
+
+#define SCMI_PAYLOAD_ARG2(payld_arr, arg1, arg2)	do {		\
+		SCMI_PAYLOAD_ARG1(payld_arr, arg1);			\
+		mmio_write_32((uintptr_t)&payld_arr[1], arg2);		\
+	} while (0)
+
+#define SCMI_PAYLOAD_ARG3(payld_arr, arg1, arg2, arg3)	do {		\
+		SCMI_PAYLOAD_ARG2(payld_arr, arg1, arg2);		\
+		mmio_write_32((uintptr_t)&payld_arr[2], arg3);		\
+	} while (0)
+
+/* Helper macros to read return values from the mailbox payload */
+#define SCMI_PAYLOAD_RET_VAL1(payld_arr, val1)				\
+		(val1) = mmio_read_32((uintptr_t)&payld_arr[0])
+
+#define SCMI_PAYLOAD_RET_VAL2(payld_arr, val1, val2)	do {		\
+		SCMI_PAYLOAD_RET_VAL1(payld_arr, val1);			\
+		(val2) = mmio_read_32((uintptr_t)&payld_arr[1]);	\
+	} while (0)
+
+#define SCMI_PAYLOAD_RET_VAL3(payld_arr, val1, val2, val3)	do {	\
+		SCMI_PAYLOAD_RET_VAL2(payld_arr, val1, val2);		\
+		(val3) = mmio_read_32((uintptr_t)&payld_arr[2]);	\
+	} while (0)
+
+#define SCMI_PAYLOAD_RET_VAL4(payld_arr, val1, val2, val3, val4)	do {	\
+		SCMI_PAYLOAD_RET_VAL3(payld_arr, val1, val2, val3);		\
+		(val4) = mmio_read_32((uintptr_t)&payld_arr[3]);		\
+	} while (0)
+
+/*
+ * Private data structure for representing the mailbox memory layout. Refer
+ * the SCMI specification for more details.
+ */
+typedef struct mailbox_mem {
+	uint32_t res_a; /* Reserved */
+	volatile uint32_t status;
+	uint64_t res_b; /* Reserved */
+	uint32_t flags;
+	volatile uint32_t len;
+	volatile uint32_t msg_header;
+	uint32_t payload[];
+} mailbox_mem_t;
+
+
+/* Private APIs for use within SCMI driver */
+void scmi_get_channel(scmi_channel_t *ch);
+void scmi_send_sync_command(scmi_channel_t *ch);
+void scmi_put_channel(scmi_channel_t *ch);
+
+static inline void validate_scmi_channel(scmi_channel_t *ch)
+{
+	assert(ch && ch->is_initialized);
+	assert(ch->info && ch->info->scmi_mbx_mem);
+}
+
+/*
+ * SCMI vendor specific protocol
+ */
+#define SCMI_SYS_VENDOR_EXT_PROTO_ID		0x80
+
+#endif /* SCMI_PRIVATE_H */
diff --git a/drivers/arm/css/scmi/scmi_pwr_dmn_proto.c b/drivers/arm/css/scmi/scmi_pwr_dmn_proto.c
new file mode 100644
index 0000000..a342aa8
--- /dev/null
+++ b/drivers/arm/css/scmi/scmi_pwr_dmn_proto.c
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/scmi.h>
+
+#include "scmi_private.h"
+
+/*
+ * API to set the SCMI power domain power state.
+ */
+int scmi_pwr_state_set(void *p, uint32_t domain_id,
+					uint32_t scmi_pwr_state)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+
+	/*
+	 * Only asynchronous mode of `set power state` command is allowed on
+	 * application processors.
+	 */
+	uint32_t pwr_state_set_msg_flag = SCMI_PWR_STATE_SET_FLAG_ASYNC;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(SCMI_PWR_DMN_PROTO_ID,
+			SCMI_PWR_STATE_SET_MSG, token);
+	mbx_mem->len = SCMI_PWR_STATE_SET_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+	SCMI_PAYLOAD_ARG3(mbx_mem->payload, pwr_state_set_msg_flag,
+						domain_id, scmi_pwr_state);
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL1(mbx_mem->payload, ret);
+	assert(mbx_mem->len == SCMI_PWR_STATE_SET_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
+
+/*
+ * API to get the SCMI power domain power state.
+ */
+int scmi_pwr_state_get(void *p, uint32_t domain_id,
+					uint32_t *scmi_pwr_state)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(SCMI_PWR_DMN_PROTO_ID,
+			SCMI_PWR_STATE_GET_MSG, token);
+	mbx_mem->len = SCMI_PWR_STATE_GET_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+	SCMI_PAYLOAD_ARG1(mbx_mem->payload, domain_id);
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL2(mbx_mem->payload, ret, *scmi_pwr_state);
+	assert(mbx_mem->len == SCMI_PWR_STATE_GET_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
diff --git a/drivers/arm/css/scmi/scmi_sys_pwr_proto.c b/drivers/arm/css/scmi/scmi_sys_pwr_proto.c
new file mode 100644
index 0000000..c8e62d1
--- /dev/null
+++ b/drivers/arm/css/scmi/scmi_sys_pwr_proto.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2017-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/scmi.h>
+
+#include "scmi_private.h"
+
+/*
+ * API to set the SCMI system power state
+ */
+int scmi_sys_pwr_state_set(void *p, uint32_t flags, uint32_t system_state)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(SCMI_SYS_PWR_PROTO_ID,
+			SCMI_SYS_PWR_STATE_SET_MSG, token);
+	mbx_mem->len = SCMI_SYS_PWR_STATE_SET_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+	SCMI_PAYLOAD_ARG2(mbx_mem->payload, flags, system_state);
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL1(mbx_mem->payload, ret);
+	assert(mbx_mem->len == SCMI_SYS_PWR_STATE_SET_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
+
+/*
+ * API to get the SCMI system power state
+ */
+int scmi_sys_pwr_state_get(void *p, uint32_t *system_state)
+{
+	mailbox_mem_t *mbx_mem;
+	unsigned int token = 0;
+	int ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(SCMI_SYS_PWR_PROTO_ID,
+			SCMI_SYS_PWR_STATE_GET_MSG, token);
+	mbx_mem->len = SCMI_SYS_PWR_STATE_GET_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+
+	scmi_send_sync_command(ch);
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL2(mbx_mem->payload, ret, *system_state);
+	assert(mbx_mem->len == SCMI_SYS_PWR_STATE_GET_RESP_LEN);
+	assert(token == SCMI_MSG_GET_TOKEN(mbx_mem->msg_header));
+
+	scmi_put_channel(ch);
+
+	return ret;
+}
diff --git a/drivers/arm/css/scmi/vendor/scmi_sq.c b/drivers/arm/css/scmi/vendor/scmi_sq.c
new file mode 100644
index 0000000..f185424
--- /dev/null
+++ b/drivers/arm/css/scmi/vendor/scmi_sq.c
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/scmi.h>
+
+#include "scmi_private.h"
+#include "scmi_sq.h"
+
+#include <sq_common.h>
+
+/* SCMI messge ID to get the available DRAM region */
+#define SCMI_VENDOR_EXT_MEMINFO_GET_MSG		0x3
+
+#define SCMI_VENDOR_EXT_MEMINFO_GET_MSG_LEN	4
+
+/*
+ * API to get the available DRAM region
+ */
+int scmi_get_draminfo(void *p, struct draminfo *info)
+{
+	mailbox_mem_t *mbx_mem;
+	int token = 0, ret;
+	scmi_channel_t *ch = (scmi_channel_t *)p;
+	struct dram_info_resp response;
+
+	validate_scmi_channel(ch);
+
+	scmi_get_channel(ch);
+
+	mbx_mem = (mailbox_mem_t *)(ch->info->scmi_mbx_mem);
+	mbx_mem->msg_header = SCMI_MSG_CREATE(SCMI_SYS_VENDOR_EXT_PROTO_ID,
+			SCMI_VENDOR_EXT_MEMINFO_GET_MSG, token);
+	mbx_mem->len = SCMI_VENDOR_EXT_MEMINFO_GET_MSG_LEN;
+	mbx_mem->flags = SCMI_FLAG_RESP_POLL;
+
+	scmi_send_sync_command(ch);
+
+	/*
+	 * Ensure that any read to the SCPI payload area is done after reading
+	 * the MHU register. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data
+	 */
+	dmbld();
+
+	/* Get the return values */
+	SCMI_PAYLOAD_RET_VAL1(mbx_mem->payload, ret);
+
+	memcpy(&response, (void *)mbx_mem->payload, sizeof(response));
+
+	scmi_put_channel(ch);
+
+	*info = response.info;
+
+	return ret;
+}
diff --git a/drivers/arm/css/scmi/vendor/scmi_sq.h b/drivers/arm/css/scmi/vendor/scmi_sq.h
new file mode 100644
index 0000000..aee1a3a
--- /dev/null
+++ b/drivers/arm/css/scmi/vendor/scmi_sq.h
@@ -0,0 +1,25 @@
+/*
+ * Copyright (c) 2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SCMI_SQ_H
+#define SCMI_SQ_H
+
+#include <stddef.h>
+#include <stdint.h>
+
+#include <sq_common.h>
+
+/* Structure to represent available DRAM region */
+struct dram_info_resp {
+	int status;
+	int reserved;
+	struct draminfo info;
+};
+
+/* API to get the available DRAM region */
+int scmi_get_draminfo(void *p, struct draminfo *info);
+
+#endif /* SCMI_SQ_H */
diff --git a/drivers/arm/css/scp/css_bom_bootloader.c b/drivers/arm/css/scp/css_bom_bootloader.c
new file mode 100644
index 0000000..74121b4
--- /dev/null
+++ b/drivers/arm/css/scp/css_bom_bootloader.c
@@ -0,0 +1,195 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/css_mhu.h>
+#include <drivers/arm/css/css_scp.h>
+#include <drivers/arm/css/css_scpi.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+/* ID of the MHU slot used for the BOM protocol */
+#define BOM_MHU_SLOT_ID		0
+
+/* Boot commands sent from AP -> SCP */
+#define BOOT_CMD_INFO	0x00
+#define BOOT_CMD_DATA	0x01
+
+/* BOM command header */
+typedef struct {
+	uint32_t id : 8;
+	uint32_t reserved : 24;
+} bom_cmd_t;
+
+typedef struct {
+	uint32_t image_size;
+	uint32_t checksum;
+} cmd_info_payload_t;
+
+/*
+ * Unlike the SCPI protocol, the boot protocol uses the same memory region
+ * for both AP -> SCP and SCP -> AP transfers; define the address of this...
+ */
+#define BOM_SHARED_MEM		PLAT_CSS_SCP_COM_SHARED_MEM_BASE
+#define BOM_CMD_HEADER		((bom_cmd_t *) BOM_SHARED_MEM)
+#define BOM_CMD_PAYLOAD		((void *) (BOM_SHARED_MEM + sizeof(bom_cmd_t)))
+
+typedef struct {
+	/* Offset from the base address of the Trusted RAM */
+	uint32_t offset;
+	uint32_t block_size;
+} cmd_data_payload_t;
+
+/*
+ * All CSS platforms load SCP_BL2/SCP_BL2U just below BL2 (this is where BL31
+ * usually resides except when ARM_BL31_IN_DRAM is
+ * set). Ensure that SCP_BL2/SCP_BL2U do not overflow into shared RAM and
+ * the fw_config.
+ */
+CASSERT(SCP_BL2_LIMIT <= BL2_BASE, assert_scp_bl2_overwrite_bl2);
+CASSERT(SCP_BL2U_LIMIT <= BL2_BASE, assert_scp_bl2u_overwrite_bl2);
+
+CASSERT(SCP_BL2_BASE >= ARM_FW_CONFIG_LIMIT, assert_scp_bl2_overflow);
+CASSERT(SCP_BL2U_BASE >= ARM_FW_CONFIG_LIMIT, assert_scp_bl2u_overflow);
+
+static void scp_boot_message_start(void)
+{
+	mhu_secure_message_start(BOM_MHU_SLOT_ID);
+}
+
+static void scp_boot_message_send(size_t payload_size)
+{
+	/* Ensure that any write to the BOM payload area is seen by SCP before
+	 * we write to the MHU register. If these 2 writes were reordered by
+	 * the CPU then SCP would read stale payload data */
+	dmbst();
+
+	/* Send command to SCP */
+	mhu_secure_message_send(BOM_MHU_SLOT_ID);
+}
+
+static uint32_t scp_boot_message_wait(size_t size)
+{
+	uint32_t mhu_status;
+
+	mhu_status = mhu_secure_message_wait();
+
+	/* Expect an SCP Boot Protocol message, reject any other protocol */
+	if (mhu_status != (1 << BOM_MHU_SLOT_ID)) {
+		ERROR("MHU: Unexpected protocol (MHU status: 0x%x)\n",
+			mhu_status);
+		panic();
+	}
+
+	/* Ensure that any read to the BOM payload area is done after reading
+	 * the MHU register. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data */
+	dmbld();
+
+	return *(uint32_t *) BOM_SHARED_MEM;
+}
+
+static void scp_boot_message_end(void)
+{
+	mhu_secure_message_end(BOM_MHU_SLOT_ID);
+}
+
+int css_scp_boot_image_xfer(void *image, unsigned int image_size)
+{
+	uint32_t response;
+	uint32_t checksum;
+	cmd_info_payload_t *cmd_info_payload;
+	cmd_data_payload_t *cmd_data_payload;
+
+	assert((uintptr_t) image == SCP_BL2_BASE);
+
+	if ((image_size == 0) || (image_size % 4 != 0)) {
+		ERROR("Invalid size for the SCP_BL2 image. Must be a multiple of "
+			"4 bytes and not zero (current size = 0x%x)\n",
+			image_size);
+		return -1;
+	}
+
+	/* Extract the checksum from the image */
+	checksum = *(uint32_t *) image;
+	image = (char *) image + sizeof(checksum);
+	image_size -= sizeof(checksum);
+
+	mhu_secure_init();
+
+	VERBOSE("Send info about the SCP_BL2 image to be transferred to SCP\n");
+
+	/*
+	 * Send information about the SCP firmware image about to be transferred
+	 * to SCP
+	 */
+	scp_boot_message_start();
+
+	BOM_CMD_HEADER->id = BOOT_CMD_INFO;
+	cmd_info_payload = BOM_CMD_PAYLOAD;
+	cmd_info_payload->image_size = image_size;
+	cmd_info_payload->checksum = checksum;
+
+	scp_boot_message_send(sizeof(*cmd_info_payload));
+#if CSS_DETECT_PRE_1_7_0_SCP
+	{
+		const uint32_t deprecated_scp_nack_cmd = 0x404;
+		uint32_t mhu_status;
+
+		VERBOSE("Detecting SCP version incompatibility\n");
+
+		mhu_status = mhu_secure_message_wait();
+		if (mhu_status == deprecated_scp_nack_cmd) {
+			ERROR("Detected an incompatible version of the SCP firmware.\n");
+			ERROR("Only versions from v1.7.0 onwards are supported.\n");
+			ERROR("Please update the SCP firmware.\n");
+			return -1;
+		}
+
+		VERBOSE("SCP version looks OK\n");
+	}
+#endif /* CSS_DETECT_PRE_1_7_0_SCP */
+	response = scp_boot_message_wait(sizeof(response));
+	scp_boot_message_end();
+
+	if (response != 0) {
+		ERROR("SCP BOOT_CMD_INFO returned error %u\n", response);
+		return -1;
+	}
+
+	VERBOSE("Transferring SCP_BL2 image to SCP\n");
+
+	/* Transfer SCP_BL2 image to SCP */
+	scp_boot_message_start();
+
+	BOM_CMD_HEADER->id = BOOT_CMD_DATA;
+	cmd_data_payload = BOM_CMD_PAYLOAD;
+	cmd_data_payload->offset = (uintptr_t) image - ARM_TRUSTED_SRAM_BASE;
+	cmd_data_payload->block_size = image_size;
+
+	scp_boot_message_send(sizeof(*cmd_data_payload));
+	response = scp_boot_message_wait(sizeof(response));
+	scp_boot_message_end();
+
+	if (response != 0) {
+		ERROR("SCP BOOT_CMD_DATA returned error %u\n", response);
+		return -1;
+	}
+
+	return 0;
+}
+
+int css_scp_boot_ready(void)
+{
+	VERBOSE("Waiting for SCP to signal it is ready to go on\n");
+
+	/* Wait for SCP to signal it's ready */
+	return scpi_wait_ready();
+}
diff --git a/drivers/arm/css/scp/css_pm_scmi.c b/drivers/arm/css/scp/css_pm_scmi.c
new file mode 100644
index 0000000..9fe8b37
--- /dev/null
+++ b/drivers/arm/css/scp/css_pm_scmi.c
@@ -0,0 +1,499 @@
+/*
+ * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/css_scp.h>
+#include <drivers/arm/css/scmi.h>
+#include <lib/mmio.h>
+#include <plat/arm/common/plat_arm.h>
+#include <plat/arm/css/common/css_pm.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+/*
+ * This file implements the SCP helper functions using SCMI protocol.
+ */
+
+/*
+ * SCMI power state parameter bit field encoding for ARM CSS platforms.
+ *
+ * 31  20 19       16 15      12 11       8 7        4 3         0
+ * +-------------------------------------------------------------+
+ * | SBZ | Max level |  Level 3 |  Level 2 |  Level 1 |  Level 0 |
+ * |     |           |   state  |   state  |   state  |   state  |
+ * +-------------------------------------------------------------+
+ *
+ * `Max level` encodes the highest level that has a valid power state
+ * encoded in the power state.
+ */
+#define SCMI_PWR_STATE_MAX_PWR_LVL_SHIFT	16
+#define SCMI_PWR_STATE_MAX_PWR_LVL_WIDTH	4
+#define SCMI_PWR_STATE_MAX_PWR_LVL_MASK		\
+				((1 << SCMI_PWR_STATE_MAX_PWR_LVL_WIDTH) - 1)
+#define SCMI_SET_PWR_STATE_MAX_PWR_LVL(_power_state, _max_level)		\
+		(_power_state) |= ((_max_level) & SCMI_PWR_STATE_MAX_PWR_LVL_MASK)\
+				<< SCMI_PWR_STATE_MAX_PWR_LVL_SHIFT
+#define SCMI_GET_PWR_STATE_MAX_PWR_LVL(_power_state)		\
+		(((_power_state) >> SCMI_PWR_STATE_MAX_PWR_LVL_SHIFT)	\
+				& SCMI_PWR_STATE_MAX_PWR_LVL_MASK)
+
+#define SCMI_PWR_STATE_LVL_WIDTH		4
+#define SCMI_PWR_STATE_LVL_MASK			\
+				((1 << SCMI_PWR_STATE_LVL_WIDTH) - 1)
+#define SCMI_SET_PWR_STATE_LVL(_power_state, _level, _level_state)		\
+		(_power_state) |= ((_level_state) & SCMI_PWR_STATE_LVL_MASK)	\
+				<< (SCMI_PWR_STATE_LVL_WIDTH * (_level))
+#define SCMI_GET_PWR_STATE_LVL(_power_state, _level)		\
+		(((_power_state) >> (SCMI_PWR_STATE_LVL_WIDTH * (_level))) &	\
+				SCMI_PWR_STATE_LVL_MASK)
+
+/*
+ * The SCMI power state enumeration for a power domain level
+ */
+typedef enum {
+	scmi_power_state_off = 0,
+	scmi_power_state_on = 1,
+	scmi_power_state_sleep = 2,
+} scmi_power_state_t;
+
+/*
+ * The global handles for invoking the SCMI driver APIs after the driver
+ * has been initialized.
+ */
+static void *scmi_handles[PLAT_ARM_SCMI_CHANNEL_COUNT];
+
+/* The global SCMI channels array */
+static scmi_channel_t scmi_channels[PLAT_ARM_SCMI_CHANNEL_COUNT];
+
+/*
+ * Channel ID for the default SCMI channel.
+ * The default channel is used to issue SYSTEM level SCMI requests and is
+ * initialized to the channel which has the boot cpu as its resource.
+ */
+static uint32_t default_scmi_channel_id;
+
+/*
+ * TODO: Allow use of channel specific lock instead of using a single lock for
+ * all the channels.
+ */
+ARM_SCMI_INSTANTIATE_LOCK;
+
+/*
+ * Function to obtain the SCMI Domain ID and SCMI Channel number from the linear
+ * core position. The SCMI Channel number is encoded in the upper 16 bits and
+ * the Domain ID is encoded in the lower 16 bits in each entry of the mapping
+ * array exported by the platform.
+ */
+static void css_scp_core_pos_to_scmi_channel(unsigned int core_pos,
+		unsigned int *scmi_domain_id, unsigned int *scmi_channel_id)
+{
+	unsigned int composite_id;
+
+	composite_id = plat_css_core_pos_to_scmi_dmn_id_map[core_pos];
+
+	*scmi_channel_id = GET_SCMI_CHANNEL_ID(composite_id);
+	*scmi_domain_id = GET_SCMI_DOMAIN_ID(composite_id);
+}
+
+/*
+ * Helper function to suspend a CPU power domain and its parent power domains
+ * if applicable.
+ */
+void css_scp_suspend(const struct psci_power_state *target_state)
+{
+	int ret;
+
+	/* At least power domain level 0 should be specified to be suspended */
+	assert(target_state->pwr_domain_state[ARM_PWR_LVL0] ==
+						ARM_LOCAL_STATE_OFF);
+
+	/* Check if power down at system power domain level is requested */
+	if (css_system_pwr_state(target_state) == ARM_LOCAL_STATE_OFF) {
+		/* Issue SCMI command for SYSTEM_SUSPEND on all SCMI channels */
+		ret = scmi_sys_pwr_state_set(
+				scmi_handles[default_scmi_channel_id],
+				SCMI_SYS_PWR_FORCEFUL_REQ, SCMI_SYS_PWR_SUSPEND);
+		if (ret != SCMI_E_SUCCESS) {
+			ERROR("SCMI system power domain suspend return 0x%x unexpected\n",
+					ret);
+			panic();
+		}
+		return;
+	}
+#if !HW_ASSISTED_COHERENCY
+	unsigned int lvl, channel_id, domain_id;
+	uint32_t scmi_pwr_state = 0;
+	/*
+	 * If we reach here, then assert that power down at system power domain
+	 * level is running.
+	 */
+	assert(css_system_pwr_state(target_state) == ARM_LOCAL_STATE_RUN);
+
+	/* For level 0, specify `scmi_power_state_sleep` as the power state */
+	SCMI_SET_PWR_STATE_LVL(scmi_pwr_state, ARM_PWR_LVL0,
+						scmi_power_state_sleep);
+
+	for (lvl = ARM_PWR_LVL1; lvl <= PLAT_MAX_PWR_LVL; lvl++) {
+		if (target_state->pwr_domain_state[lvl] == ARM_LOCAL_STATE_RUN)
+			break;
+
+		assert(target_state->pwr_domain_state[lvl] ==
+							ARM_LOCAL_STATE_OFF);
+		/*
+		 * Specify `scmi_power_state_off` as power state for higher
+		 * levels.
+		 */
+		SCMI_SET_PWR_STATE_LVL(scmi_pwr_state, lvl,
+						scmi_power_state_off);
+	}
+
+	SCMI_SET_PWR_STATE_MAX_PWR_LVL(scmi_pwr_state, lvl - 1);
+
+	css_scp_core_pos_to_scmi_channel(plat_my_core_pos(),
+			&domain_id, &channel_id);
+	ret = scmi_pwr_state_set(scmi_handles[channel_id],
+		domain_id, scmi_pwr_state);
+
+	if (ret != SCMI_E_SUCCESS) {
+		ERROR("SCMI set power state command return 0x%x unexpected\n",
+				ret);
+		panic();
+	}
+#endif
+}
+
+/*
+ * Helper function to turn off a CPU power domain and its parent power domains
+ * if applicable.
+ */
+void css_scp_off(const struct psci_power_state *target_state)
+{
+	unsigned int lvl = 0, channel_id, domain_id;
+	int ret;
+	uint32_t scmi_pwr_state = 0;
+
+	/* At-least the CPU level should be specified to be OFF */
+	assert(target_state->pwr_domain_state[ARM_PWR_LVL0] ==
+							ARM_LOCAL_STATE_OFF);
+
+	/* PSCI CPU OFF cannot be used to turn OFF system power domain */
+	assert(css_system_pwr_state(target_state) == ARM_LOCAL_STATE_RUN);
+
+	for (; lvl <= PLAT_MAX_PWR_LVL; lvl++) {
+		if (target_state->pwr_domain_state[lvl] == ARM_LOCAL_STATE_RUN)
+			break;
+
+		assert(target_state->pwr_domain_state[lvl] ==
+							ARM_LOCAL_STATE_OFF);
+		SCMI_SET_PWR_STATE_LVL(scmi_pwr_state, lvl,
+				scmi_power_state_off);
+	}
+
+	SCMI_SET_PWR_STATE_MAX_PWR_LVL(scmi_pwr_state, lvl - 1);
+
+	css_scp_core_pos_to_scmi_channel(plat_my_core_pos(),
+			&domain_id, &channel_id);
+	ret = scmi_pwr_state_set(scmi_handles[channel_id],
+		domain_id, scmi_pwr_state);
+	if (ret != SCMI_E_QUEUED && ret != SCMI_E_SUCCESS) {
+		ERROR("SCMI set power state command return 0x%x unexpected\n",
+				ret);
+		panic();
+	}
+}
+
+/*
+ * Helper function to turn ON a CPU power domain and its parent power domains
+ * if applicable.
+ */
+void css_scp_on(u_register_t mpidr)
+{
+	unsigned int lvl = 0, channel_id, core_pos, domain_id;
+	int ret;
+	uint32_t scmi_pwr_state = 0;
+
+	for (; lvl <= PLAT_MAX_PWR_LVL; lvl++)
+		SCMI_SET_PWR_STATE_LVL(scmi_pwr_state, lvl,
+				scmi_power_state_on);
+
+	SCMI_SET_PWR_STATE_MAX_PWR_LVL(scmi_pwr_state, lvl - 1);
+
+	core_pos = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+	assert(core_pos < PLATFORM_CORE_COUNT);
+
+	css_scp_core_pos_to_scmi_channel(core_pos, &domain_id,
+			&channel_id);
+	ret = scmi_pwr_state_set(scmi_handles[channel_id],
+		domain_id, scmi_pwr_state);
+	if (ret != SCMI_E_QUEUED && ret != SCMI_E_SUCCESS) {
+		ERROR("SCMI set power state command return 0x%x unexpected\n",
+				ret);
+		panic();
+	}
+}
+
+/*
+ * Helper function to get the power state of a power domain node as reported
+ * by the SCP.
+ */
+int css_scp_get_power_state(u_register_t mpidr, unsigned int power_level)
+{
+	int ret;
+	uint32_t scmi_pwr_state = 0, lvl_state;
+	unsigned int channel_id, cpu_idx, domain_id;
+
+	/* We don't support get power state at the system power domain level */
+	if ((power_level > PLAT_MAX_PWR_LVL) ||
+			(power_level == CSS_SYSTEM_PWR_DMN_LVL)) {
+		WARN("Invalid power level %u specified for SCMI get power state\n",
+				power_level);
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	cpu_idx = (unsigned int)plat_core_pos_by_mpidr(mpidr);
+	assert(cpu_idx < PLATFORM_CORE_COUNT);
+
+	css_scp_core_pos_to_scmi_channel(cpu_idx, &domain_id, &channel_id);
+	ret = scmi_pwr_state_get(scmi_handles[channel_id],
+		domain_id, &scmi_pwr_state);
+
+	if (ret != SCMI_E_SUCCESS) {
+		WARN("SCMI get power state command return 0x%x unexpected\n",
+				ret);
+		return PSCI_E_INVALID_PARAMS;
+	}
+
+	/*
+	 * Find the maximum power level described in the get power state
+	 * command. If it is less than the requested power level, then assume
+	 * the requested power level is ON.
+	 */
+	if (SCMI_GET_PWR_STATE_MAX_PWR_LVL(scmi_pwr_state) < power_level)
+		return HW_ON;
+
+	lvl_state = SCMI_GET_PWR_STATE_LVL(scmi_pwr_state, power_level);
+	if (lvl_state == scmi_power_state_on)
+		return HW_ON;
+
+	assert((lvl_state == scmi_power_state_off) ||
+				(lvl_state == scmi_power_state_sleep));
+	return HW_OFF;
+}
+
+/*
+ * Callback function to raise a SGI designated to trigger the CPU power down
+ * sequence on all the online secondary cores.
+ */
+static void css_raise_pwr_down_interrupt(u_register_t mpidr)
+{
+#if CSS_SYSTEM_GRACEFUL_RESET
+	plat_ic_raise_el3_sgi(CSS_CPU_PWR_DOWN_REQ_INTR, mpidr);
+#endif
+}
+
+void __dead2 css_scp_system_off(int state)
+{
+	int ret;
+
+	/*
+	 * Before issuing the system power down command, set the trusted mailbox
+	 * to 0. This will ensure that in the case of a warm/cold reset, the
+	 * primary CPU executes from the cold boot sequence.
+	 */
+	mmio_write_64(PLAT_ARM_TRUSTED_MAILBOX_BASE, 0U);
+
+	/*
+	 * Send powerdown request to online secondary core(s)
+	 */
+	ret = psci_stop_other_cores(0, css_raise_pwr_down_interrupt);
+	if (ret != PSCI_E_SUCCESS) {
+		ERROR("Failed to powerdown secondary core(s)\n");
+	}
+
+	/*
+	 * Disable GIC CPU interface to prevent pending interrupt from waking
+	 * up the AP from WFI.
+	 */
+	plat_arm_gic_cpuif_disable();
+	plat_arm_gic_redistif_off();
+
+	/*
+	 * Issue SCMI command. First issue a graceful
+	 * request and if that fails force the request.
+	 */
+	ret = scmi_sys_pwr_state_set(scmi_handles[default_scmi_channel_id],
+			SCMI_SYS_PWR_FORCEFUL_REQ,
+			state);
+
+	if (ret != SCMI_E_SUCCESS) {
+		ERROR("SCMI system power state set 0x%x returns unexpected 0x%x\n",
+			state, ret);
+		panic();
+	}
+
+	/* Powerdown of primary core */
+	psci_pwrdown_cpu(PLAT_MAX_PWR_LVL);
+	wfi();
+	ERROR("CSS set power state: operation not handled.\n");
+	panic();
+}
+
+/*
+ * Helper function to shutdown the system via SCMI.
+ */
+void __dead2 css_scp_sys_shutdown(void)
+{
+	css_scp_system_off(SCMI_SYS_PWR_SHUTDOWN);
+}
+
+/*
+ * Helper function to reset the system via SCMI.
+ */
+void __dead2 css_scp_sys_reboot(void)
+{
+	css_scp_system_off(SCMI_SYS_PWR_COLD_RESET);
+}
+
+static int scmi_ap_core_init(scmi_channel_t *ch)
+{
+#if PROGRAMMABLE_RESET_ADDRESS
+	uint32_t version;
+	int ret;
+
+	ret = scmi_proto_version(ch, SCMI_AP_CORE_PROTO_ID, &version);
+	if (ret != SCMI_E_SUCCESS) {
+		WARN("SCMI AP core protocol version message failed\n");
+		return -1;
+	}
+
+	if (!is_scmi_version_compatible(SCMI_AP_CORE_PROTO_VER, version)) {
+		WARN("SCMI AP core protocol version 0x%x incompatible with driver version 0x%x\n",
+			version, SCMI_AP_CORE_PROTO_VER);
+		return -1;
+	}
+	INFO("SCMI AP core protocol version 0x%x detected\n", version);
+#endif
+	return 0;
+}
+
+void __init plat_arm_pwrc_setup(void)
+{
+	unsigned int composite_id, idx;
+
+	for (idx = 0; idx < PLAT_ARM_SCMI_CHANNEL_COUNT; idx++) {
+		INFO("Initializing SCMI driver on channel %d\n", idx);
+
+		scmi_channels[idx].info = plat_css_get_scmi_info(idx);
+		scmi_channels[idx].lock = ARM_SCMI_LOCK_GET_INSTANCE;
+		scmi_handles[idx] = scmi_init(&scmi_channels[idx]);
+
+		if (scmi_handles[idx] == NULL) {
+			ERROR("SCMI Initialization failed on channel %d\n", idx);
+			panic();
+		}
+
+		if (scmi_ap_core_init(&scmi_channels[idx]) < 0) {
+			ERROR("SCMI AP core protocol initialization failed\n");
+			panic();
+		}
+	}
+
+	composite_id = plat_css_core_pos_to_scmi_dmn_id_map[plat_my_core_pos()];
+	default_scmi_channel_id = GET_SCMI_CHANNEL_ID(composite_id);
+}
+
+/******************************************************************************
+ * This function overrides the default definition for ARM platforms. Initialize
+ * the SCMI driver, query capability via SCMI and modify the PSCI capability
+ * based on that.
+ *****************************************************************************/
+const plat_psci_ops_t *css_scmi_override_pm_ops(plat_psci_ops_t *ops)
+{
+	uint32_t msg_attr;
+	int ret;
+	void *scmi_handle = scmi_handles[default_scmi_channel_id];
+
+	assert(scmi_handle);
+
+	/* Check that power domain POWER_STATE_SET message is supported */
+	ret = scmi_proto_msg_attr(scmi_handle, SCMI_PWR_DMN_PROTO_ID,
+				SCMI_PWR_STATE_SET_MSG, &msg_attr);
+	if (ret != SCMI_E_SUCCESS) {
+		ERROR("Set power state command is not supported by SCMI\n");
+		panic();
+	}
+
+	/*
+	 * Don't support PSCI NODE_HW_STATE call if SCMI doesn't support
+	 * POWER_STATE_GET message.
+	 */
+	ret = scmi_proto_msg_attr(scmi_handle, SCMI_PWR_DMN_PROTO_ID,
+				SCMI_PWR_STATE_GET_MSG, &msg_attr);
+	if (ret != SCMI_E_SUCCESS)
+		ops->get_node_hw_state = NULL;
+
+	/* Check if the SCMI SYSTEM_POWER_STATE_SET message is supported */
+	ret = scmi_proto_msg_attr(scmi_handle, SCMI_SYS_PWR_PROTO_ID,
+				SCMI_SYS_PWR_STATE_SET_MSG, &msg_attr);
+	if (ret != SCMI_E_SUCCESS) {
+		/* System power management operations are not supported */
+		ops->system_off = NULL;
+		ops->system_reset = NULL;
+		ops->get_sys_suspend_power_state = NULL;
+	} else {
+		if (!(msg_attr & SCMI_SYS_PWR_SUSPEND_SUPPORTED)) {
+			/*
+			 * System power management protocol is available, but
+			 * it does not support SYSTEM SUSPEND.
+			 */
+			ops->get_sys_suspend_power_state = NULL;
+		}
+		if (!(msg_attr & SCMI_SYS_PWR_WARM_RESET_SUPPORTED)) {
+			/*
+			 * WARM reset is not available.
+			 */
+			ops->system_reset2 = NULL;
+		}
+	}
+
+	return ops;
+}
+
+int css_system_reset2(int is_vendor, int reset_type, u_register_t cookie)
+{
+	if (is_vendor || (reset_type != PSCI_RESET2_SYSTEM_WARM_RESET))
+		return PSCI_E_INVALID_PARAMS;
+
+	css_scp_system_off(SCMI_SYS_PWR_WARM_RESET);
+	/*
+	 * css_scp_system_off cannot return (it is a __dead function),
+	 * but css_system_reset2 has to return some value, even in
+	 * this case.
+	 */
+	return 0;
+}
+
+#if PROGRAMMABLE_RESET_ADDRESS
+void plat_arm_program_trusted_mailbox(uintptr_t address)
+{
+	int ret, i;
+
+	for (i = 0; i < PLAT_ARM_SCMI_CHANNEL_COUNT; i++) {
+		assert(scmi_handles[i]);
+
+		ret = scmi_ap_core_set_reset_addr(scmi_handles[i], address,
+				SCMI_AP_CORE_LOCK_ATTR);
+		if (ret != SCMI_E_SUCCESS) {
+			ERROR("CSS: Failed to program reset address: %d\n", ret);
+			panic();
+		}
+	}
+}
+#endif
diff --git a/drivers/arm/css/scp/css_pm_scpi.c b/drivers/arm/css/scp/css_pm_scpi.c
new file mode 100644
index 0000000..b4019ce
--- /dev/null
+++ b/drivers/arm/css/scp/css_pm_scpi.c
@@ -0,0 +1,165 @@
+/*
+ * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/css_scp.h>
+#include <drivers/arm/css/css_scpi.h>
+#include <plat/arm/common/plat_arm.h>
+#include <plat/arm/css/common/css_pm.h>
+
+/*
+ * This file implements the SCP power management functions using SCPI protocol.
+ */
+
+/*
+ * Helper function to inform power down state to SCP.
+ */
+void css_scp_suspend(const struct psci_power_state *target_state)
+{
+	uint32_t cluster_state = scpi_power_on;
+	uint32_t system_state = scpi_power_on;
+
+	/* Check if power down at system power domain level is requested */
+	if (css_system_pwr_state(target_state) == ARM_LOCAL_STATE_OFF)
+		system_state = scpi_power_retention;
+
+	/* Cluster is to be turned off, so disable coherency */
+	if (CSS_CLUSTER_PWR_STATE(target_state) == ARM_LOCAL_STATE_OFF)
+		cluster_state = scpi_power_off;
+
+	/*
+	 * Ask the SCP to power down the appropriate components depending upon
+	 * their state.
+	 */
+	scpi_set_css_power_state(read_mpidr_el1(),
+				 scpi_power_off,
+				 cluster_state,
+				 system_state);
+}
+
+/*
+ * Helper function to turn off a CPU power domain and its parent power domains
+ * if applicable. Since SCPI doesn't differentiate between OFF and suspend, we
+ * call the suspend helper here.
+ */
+void css_scp_off(const struct psci_power_state *target_state)
+{
+	css_scp_suspend(target_state);
+}
+
+/*
+ * Helper function to turn ON a CPU power domain and its parent power domains
+ * if applicable.
+ */
+void css_scp_on(u_register_t mpidr)
+{
+	/*
+	 * SCP takes care of powering up parent power domains so we
+	 * only need to care about level 0
+	 */
+	scpi_set_css_power_state(mpidr, scpi_power_on, scpi_power_on,
+				 scpi_power_on);
+}
+
+/*
+ * Helper function to get the power state of a power domain node as reported
+ * by the SCP.
+ */
+int css_scp_get_power_state(u_register_t mpidr, unsigned int power_level)
+{
+	int rc, element;
+	unsigned int cpu_state, cluster_state;
+
+	/*
+	 * The format of 'power_level' is implementation-defined, but 0 must
+	 * mean a CPU. We also allow 1 to denote the cluster
+	 */
+	if (power_level != ARM_PWR_LVL0 && power_level != ARM_PWR_LVL1)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Query SCP */
+	rc = scpi_get_css_power_state(mpidr, &cpu_state, &cluster_state);
+	if (rc != 0)
+		return PSCI_E_INVALID_PARAMS;
+
+	/* Map power states of CPU and cluster to expected PSCI return codes */
+	if (power_level == ARM_PWR_LVL0) {
+		/*
+		 * The CPU state returned by SCP is an 8-bit bit mask
+		 * corresponding to each CPU in the cluster
+		 */
+#if ARM_PLAT_MT
+		/*
+		 * The current SCPI driver only caters for single-threaded
+		 * platforms. Hence we ignore the thread ID (which is always 0)
+		 * for such platforms.
+		 */
+		element = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+#else
+		element = mpidr & MPIDR_AFFLVL_MASK;
+#endif  /* ARM_PLAT_MT */
+		return CSS_CPU_PWR_STATE(cpu_state, element) ==
+			CSS_CPU_PWR_STATE_ON ? HW_ON : HW_OFF;
+	} else {
+		assert(cluster_state == CSS_CLUSTER_PWR_STATE_ON ||
+				cluster_state == CSS_CLUSTER_PWR_STATE_OFF);
+		return cluster_state == CSS_CLUSTER_PWR_STATE_ON ? HW_ON :
+			HW_OFF;
+	}
+}
+
+/*
+ * Helper function to shutdown the system via SCPI.
+ */
+void __dead2 css_scp_sys_shutdown(void)
+{
+	uint32_t response;
+
+	/*
+	 * Disable GIC CPU interface to prevent pending interrupt
+	 * from waking up the AP from WFI.
+	 */
+	plat_arm_gic_cpuif_disable();
+
+	/* Send the power down request to the SCP */
+	response = scpi_sys_power_state(scpi_system_shutdown);
+
+	if (response != SCP_OK) {
+		ERROR("CSS System Off: SCP error %u.\n", response);
+		panic();
+	}
+	wfi();
+	ERROR("CSS System Off: operation not handled.\n");
+	panic();
+}
+
+/*
+ * Helper function to reset the system via SCPI.
+ */
+void __dead2 css_scp_sys_reboot(void)
+{
+	uint32_t response;
+
+	/*
+	 * Disable GIC CPU interface to prevent pending interrupt
+	 * from waking up the AP from WFI.
+	 */
+	plat_arm_gic_cpuif_disable();
+
+	/* Send the system reset request to the SCP */
+	response = scpi_sys_power_state(scpi_system_reboot);
+
+	if (response != SCP_OK) {
+		ERROR("CSS System Reset: SCP error %u.\n", response);
+		panic();
+	}
+	wfi();
+	ERROR("CSS System Reset: operation not handled.\n");
+	panic();
+}
diff --git a/drivers/arm/css/scp/css_sds.c b/drivers/arm/css/scp/css_sds.c
new file mode 100644
index 0000000..e42ee10
--- /dev/null
+++ b/drivers/arm/css/scp/css_sds.c
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2014-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/css_scp.h>
+#include <drivers/arm/css/sds.h>
+#include <drivers/delay_timer.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+int css_scp_boot_image_xfer(void *image, unsigned int image_size)
+{
+	int ret;
+	unsigned int image_offset, image_flags;
+
+	ret = sds_init();
+	if (ret != SDS_OK) {
+		ERROR("SCP SDS initialization failed\n");
+		panic();
+	}
+
+	VERBOSE("Writing SCP image metadata\n");
+	image_offset = (uintptr_t) image - ARM_TRUSTED_SRAM_BASE;
+	ret = sds_struct_write(SDS_SCP_IMG_STRUCT_ID, SDS_SCP_IMG_ADDR_OFFSET,
+			&image_offset, SDS_SCP_IMG_ADDR_SIZE,
+			SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK)
+		goto sds_fail;
+
+	ret = sds_struct_write(SDS_SCP_IMG_STRUCT_ID, SDS_SCP_IMG_SIZE_OFFSET,
+			&image_size, SDS_SCP_IMG_SIZE_SIZE,
+			SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK)
+		goto sds_fail;
+
+	VERBOSE("Marking SCP image metadata as valid\n");
+	image_flags = SDS_SCP_IMG_VALID_FLAG_BIT;
+	ret = sds_struct_write(SDS_SCP_IMG_STRUCT_ID, SDS_SCP_IMG_FLAG_OFFSET,
+			&image_flags, SDS_SCP_IMG_FLAG_SIZE,
+			SDS_ACCESS_MODE_NON_CACHED);
+	if (ret != SDS_OK)
+		goto sds_fail;
+
+	return 0;
+sds_fail:
+	ERROR("SCP SDS write to SCP IMG struct failed\n");
+	panic();
+}
+
+/*
+ * API to wait for SCP to signal till it's ready after booting the transferred
+ * image.
+ */
+int css_scp_boot_ready(void)
+{
+	uint32_t scp_feature_availability_flags;
+	int ret, retry = CSS_SCP_READY_10US_RETRIES;
+
+
+	VERBOSE("Waiting for SCP RAM to complete its initialization process\n");
+
+	/* Wait for the SCP RAM Firmware to complete its initialization process */
+	while (retry > 0) {
+		ret = sds_struct_read(SDS_FEATURE_AVAIL_STRUCT_ID, 0,
+				&scp_feature_availability_flags,
+				SDS_FEATURE_AVAIL_SIZE,
+				SDS_ACCESS_MODE_NON_CACHED);
+		if (ret == SDS_ERR_STRUCT_NOT_FINALIZED)
+			continue;
+
+		if (ret != SDS_OK) {
+			ERROR(" sds_struct_read failed\n");
+			panic();
+		}
+
+		if (scp_feature_availability_flags &
+				SDS_FEATURE_AVAIL_SCP_RAM_READY_BIT)
+			return 0;
+
+		udelay(10);
+		retry--;
+	}
+
+	ERROR("Timeout of %d ms expired waiting for SCP RAM Ready flag\n",
+			CSS_SCP_READY_10US_RETRIES/100);
+
+	plat_panic_handler();
+}
diff --git a/drivers/arm/css/scpi/css_scpi.c b/drivers/arm/css/scpi/css_scpi.c
new file mode 100644
index 0000000..416356b
--- /dev/null
+++ b/drivers/arm/css/scpi/css_scpi.c
@@ -0,0 +1,272 @@
+/*
+ * Copyright (c) 2014-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/css_mhu.h>
+#include <drivers/arm/css/css_scpi.h>
+#include <lib/utils.h>
+#include <plat/common/platform.h>
+#include <platform_def.h>
+
+#define SCPI_SHARED_MEM_SCP_TO_AP	PLAT_CSS_SCP_COM_SHARED_MEM_BASE
+#define SCPI_SHARED_MEM_AP_TO_SCP	(PLAT_CSS_SCP_COM_SHARED_MEM_BASE \
+								 + 0x100)
+
+/* Header and payload addresses for commands from AP to SCP */
+#define SCPI_CMD_HEADER_AP_TO_SCP		\
+	((scpi_cmd_t *) SCPI_SHARED_MEM_AP_TO_SCP)
+#define SCPI_CMD_PAYLOAD_AP_TO_SCP		\
+	((void *) (SCPI_SHARED_MEM_AP_TO_SCP + sizeof(scpi_cmd_t)))
+
+/* Header and payload addresses for responses from SCP to AP */
+#define SCPI_RES_HEADER_SCP_TO_AP \
+	((scpi_cmd_t *) SCPI_SHARED_MEM_SCP_TO_AP)
+#define SCPI_RES_PAYLOAD_SCP_TO_AP \
+	((void *) (SCPI_SHARED_MEM_SCP_TO_AP + sizeof(scpi_cmd_t)))
+
+/* ID of the MHU slot used for the SCPI protocol */
+#define SCPI_MHU_SLOT_ID		0
+
+static void scpi_secure_message_start(void)
+{
+	mhu_secure_message_start(SCPI_MHU_SLOT_ID);
+}
+
+static void scpi_secure_message_send(size_t payload_size)
+{
+	/*
+	 * Ensure that any write to the SCPI payload area is seen by SCP before
+	 * we write to the MHU register. If these 2 writes were reordered by
+	 * the CPU then SCP would read stale payload data
+	 */
+	dmbst();
+
+	mhu_secure_message_send(SCPI_MHU_SLOT_ID);
+}
+
+static int scpi_secure_message_receive(scpi_cmd_t *cmd)
+{
+	uint32_t mhu_status;
+
+	assert(cmd != NULL);
+
+	mhu_status = mhu_secure_message_wait();
+
+	/* Expect an SCPI message, reject any other protocol */
+	if (mhu_status != (1 << SCPI_MHU_SLOT_ID)) {
+		ERROR("MHU: Unexpected protocol (MHU status: 0x%x)\n",
+			mhu_status);
+		return -1;
+	}
+
+	/*
+	 * Ensure that any read to the SCPI payload area is done after reading
+	 * the MHU register. If these 2 reads were reordered then the CPU would
+	 * read invalid payload data
+	 */
+	dmbld();
+
+	memcpy(cmd, (void *) SCPI_SHARED_MEM_SCP_TO_AP, sizeof(*cmd));
+
+	return 0;
+}
+
+static void scpi_secure_message_end(void)
+{
+	mhu_secure_message_end(SCPI_MHU_SLOT_ID);
+}
+
+int scpi_wait_ready(void)
+{
+	scpi_cmd_t scpi_cmd;
+	int rc;
+
+	VERBOSE("Waiting for SCP_READY command...\n");
+
+	/* Get a message from the SCP */
+	scpi_secure_message_start();
+	rc = scpi_secure_message_receive(&scpi_cmd);
+	scpi_secure_message_end();
+
+	/* If no message was received, don't send a response */
+	if (rc != 0)
+		return rc;
+
+	/* We are expecting 'SCP Ready', produce correct error if it's not */
+	scpi_status_t status = SCP_OK;
+	if (scpi_cmd.id != SCPI_CMD_SCP_READY) {
+		ERROR("Unexpected SCP command: expected command #%u, got command #%u\n",
+		      SCPI_CMD_SCP_READY, scpi_cmd.id);
+		status = SCP_E_SUPPORT;
+	} else if (scpi_cmd.size != 0) {
+		ERROR("SCP_READY command has incorrect size: expected 0, got %u\n",
+		      scpi_cmd.size);
+		status = SCP_E_SIZE;
+	}
+
+	VERBOSE("Sending response for SCP_READY command\n");
+
+	/*
+	 * Send our response back to SCP.
+	 * We are using the same SCPI header, just update the status field.
+	 */
+	scpi_cmd.status = status;
+	scpi_secure_message_start();
+	memcpy((void *) SCPI_SHARED_MEM_AP_TO_SCP, &scpi_cmd, sizeof(scpi_cmd));
+	scpi_secure_message_send(0);
+	scpi_secure_message_end();
+
+	return status == SCP_OK ? 0 : -1;
+}
+
+void scpi_set_css_power_state(unsigned int mpidr,
+		scpi_power_state_t cpu_state, scpi_power_state_t cluster_state,
+		scpi_power_state_t css_state)
+{
+	scpi_cmd_t *cmd;
+	uint32_t state = 0;
+	uint32_t *payload_addr;
+
+#if ARM_PLAT_MT
+	/*
+	 * The current SCPI driver only caters for single-threaded platforms.
+	 * Hence we ignore the thread ID (which is always 0) for such platforms.
+	 */
+	state |= (mpidr >> MPIDR_AFF1_SHIFT) & 0x0f;	/* CPU ID */
+	state |= ((mpidr >> MPIDR_AFF2_SHIFT) & 0x0f) << 4;	/* Cluster ID */
+#else
+	state |= mpidr & 0x0f;	/* CPU ID */
+	state |= (mpidr & 0xf00) >> 4;	/* Cluster ID */
+#endif /* ARM_PLAT_MT */
+
+	state |= cpu_state << 8;
+	state |= cluster_state << 12;
+	state |= css_state << 16;
+
+	scpi_secure_message_start();
+
+	/* Populate the command header */
+	cmd = SCPI_CMD_HEADER_AP_TO_SCP;
+	cmd->id = SCPI_CMD_SET_CSS_POWER_STATE;
+	cmd->set = SCPI_SET_NORMAL;
+	cmd->sender = 0;
+	cmd->size = sizeof(state);
+	/* Populate the command payload */
+	payload_addr = SCPI_CMD_PAYLOAD_AP_TO_SCP;
+	*payload_addr = state;
+	scpi_secure_message_send(sizeof(state));
+	/*
+	 * SCP does not reply to this command in order to avoid MHU interrupts
+	 * from the sender, which could interfere with its power state request.
+	 */
+
+	scpi_secure_message_end();
+}
+
+/*
+ * Query and obtain CSS power state from SCP.
+ *
+ * In response to the query, SCP returns power states of all CPUs in all
+ * clusters of the system. The returned response is then filtered based on the
+ * supplied MPIDR. Power states of requested cluster and CPUs within are updated
+ * via supplied non-NULL pointer arguments.
+ *
+ * Returns 0 on success, or -1 on errors.
+ */
+int scpi_get_css_power_state(unsigned int mpidr, unsigned int *cpu_state_p,
+		unsigned int *cluster_state_p)
+{
+	scpi_cmd_t *cmd;
+	scpi_cmd_t response;
+	int power_state, cpu, cluster, rc = -1;
+
+	/*
+	 * Extract CPU and cluster membership of the given MPIDR. SCPI caters
+	 * for only up to 0xf clusters, and 8 CPUs per cluster
+	 */
+#if ARM_PLAT_MT
+	/*
+	 * The current SCPI driver only caters for single-threaded platforms.
+	 * Hence we ignore the thread ID (which is always 0) for such platforms.
+	 */
+	cpu = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+	cluster = (mpidr >> MPIDR_AFF2_SHIFT) & MPIDR_AFFLVL_MASK;
+#else
+	cpu = mpidr & MPIDR_AFFLVL_MASK;
+	cluster = (mpidr >> MPIDR_AFF1_SHIFT) & MPIDR_AFFLVL_MASK;
+#endif  /* ARM_PLAT_MT */
+	if (cpu >= 8 || cluster >= 0xf)
+		return -1;
+
+	scpi_secure_message_start();
+
+	/* Populate request headers */
+	zeromem(SCPI_CMD_HEADER_AP_TO_SCP, sizeof(*cmd));
+	cmd = SCPI_CMD_HEADER_AP_TO_SCP;
+	cmd->id = SCPI_CMD_GET_CSS_POWER_STATE;
+
+	/*
+	 * Send message and wait for SCP's response
+	 */
+	scpi_secure_message_send(0);
+	if (scpi_secure_message_receive(&response) != 0)
+		goto exit;
+
+	if (response.status != SCP_OK)
+		goto exit;
+
+	/* Validate SCP response */
+	if (!CHECK_RESPONSE(response, cluster))
+		goto exit;
+
+	/* Extract power states for required cluster */
+	power_state = *(((uint16_t *) SCPI_RES_PAYLOAD_SCP_TO_AP) + cluster);
+	if (CLUSTER_ID(power_state) != cluster)
+		goto exit;
+
+	/* Update power state via pointers */
+	if (cluster_state_p)
+		*cluster_state_p = CLUSTER_POWER_STATE(power_state);
+	if (cpu_state_p)
+		*cpu_state_p = CPU_POWER_STATE(power_state);
+	rc = 0;
+
+exit:
+	scpi_secure_message_end();
+	return rc;
+}
+
+uint32_t scpi_sys_power_state(scpi_system_state_t system_state)
+{
+	scpi_cmd_t *cmd;
+	uint8_t *payload_addr;
+	scpi_cmd_t response;
+
+	scpi_secure_message_start();
+
+	/* Populate the command header */
+	cmd = SCPI_CMD_HEADER_AP_TO_SCP;
+	cmd->id = SCPI_CMD_SYS_POWER_STATE;
+	cmd->set = 0;
+	cmd->sender = 0;
+	cmd->size = sizeof(*payload_addr);
+	/* Populate the command payload */
+	payload_addr = SCPI_CMD_PAYLOAD_AP_TO_SCP;
+	*payload_addr = system_state & 0xff;
+	scpi_secure_message_send(sizeof(*payload_addr));
+
+	/* If no response is received, fill in an error status */
+	if (scpi_secure_message_receive(&response) != 0)
+		response.status = SCP_E_TIMEOUT;
+
+	scpi_secure_message_end();
+
+	return response.status;
+}
diff --git a/drivers/arm/css/sds/aarch32/sds_helpers.S b/drivers/arm/css/sds/aarch32/sds_helpers.S
new file mode 100644
index 0000000..13ff0e1
--- /dev/null
+++ b/drivers/arm/css/sds/aarch32/sds_helpers.S
@@ -0,0 +1,64 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <drivers/arm/css/sds.h>
+#include <platform_def.h>
+
+#include "../sds_private.h"
+
+	.globl	sds_get_primary_cpu_id
+
+	/*
+	 * int sds_get_primary_cpu_id(void);
+	 * Return the primary CPU ID from SDS Structure
+	 * Returns CPUID on success or -1 on failure
+	 */
+func sds_get_primary_cpu_id
+	ldr	r0, =PLAT_ARM_SDS_MEM_BASE
+	ldr	r2, =SDS_REGION_SIGNATURE
+	ldr	r1, [r0]
+	ubfx	r3, r1, #0, #16
+
+	/* Check if the SDS region signature found */
+	cmp	r2, r3
+	bne	2f
+
+	/* Get the structure count from region descriptor in r1 */
+	ubfx	r1, r1, #SDS_REGION_STRUCT_COUNT_SHIFT, #SDS_REGION_STRUCT_COUNT_WIDTH
+	cmp	r1, #0
+	beq	2f
+	add	r0, r0, #SDS_REGION_DESC_SIZE
+
+	/* Initialize the loop iterator count in r3 */
+	mov	r3, #0
+loop_begin:
+	ldrh	r2, [r0]
+	cmp	r2, #SDS_AP_CPU_INFO_STRUCT_ID
+	bne	continue_loop
+
+	/* We have found the required structure */
+	ldr	r0, [r0,#(SDS_HEADER_SIZE + SDS_AP_CPU_INFO_PRIMARY_CPUID_OFFSET)]
+	bx	lr
+continue_loop:
+	/* Increment the loop counter and exit loop if counter == structure count */
+	add	r3, r3, #0x1
+	cmp	r1, r3
+	beq	2f
+
+	/* Read the 2nd word in header */
+	ldr	r2, [r0,#4]
+	/* Get the structure size from header */
+	ubfx	r2, r2, #SDS_HEADER_STRUCT_SIZE_SHIFT, #SDS_HEADER_STRUCT_SIZE_WIDTH
+	/* Add the structure size and SDS HEADER SIZE to point to next header */
+	add	r2, r2, #SDS_HEADER_SIZE
+	add	r0, r0, r2
+	b	loop_begin
+2:
+	mov	r0, #0xffffffff
+	bx	lr
+endfunc sds_get_primary_cpu_id
diff --git a/drivers/arm/css/sds/aarch64/sds_helpers.S b/drivers/arm/css/sds/aarch64/sds_helpers.S
new file mode 100644
index 0000000..3256c2b
--- /dev/null
+++ b/drivers/arm/css/sds/aarch64/sds_helpers.S
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <asm_macros.S>
+#include <drivers/arm/css/sds.h>
+#include <platform_def.h>
+
+#include "../sds_private.h"
+
+	.globl	sds_get_primary_cpu_id
+
+	/*
+	 * int sds_get_primary_cpu_id(void);
+	 * Return the primary CPI ID from SDS Structure
+	 * Returns CPUID on success or -1 on failure
+	 */
+func sds_get_primary_cpu_id
+	mov_imm	x0, PLAT_ARM_SDS_MEM_BASE
+	mov	w2, #SDS_REGION_SIGNATURE
+	ldr	w1, [x0]
+
+	/* Check if the SDS region signature found */
+	cmp	w2, w1, uxth
+	b.ne	2f
+
+	/* Get the structure count from region descriptor in `w1 */
+	ubfx	w1, w1, #SDS_REGION_STRUCT_COUNT_SHIFT, #SDS_REGION_STRUCT_COUNT_WIDTH
+	cbz	w1, 2f
+	add	x0, x0, #SDS_REGION_DESC_SIZE
+
+	/* Initialize the loop iterator count in w3 */
+	mov	w3, #0
+loop_begin:
+	ldrh	w2, [x0]
+	cmp	w2, #SDS_AP_CPU_INFO_STRUCT_ID
+	b.ne	continue_loop
+
+	/* We have found the required structure */
+	ldr	w0, [x0,#(SDS_HEADER_SIZE + SDS_AP_CPU_INFO_PRIMARY_CPUID_OFFSET)]
+	ret
+continue_loop:
+	/* Increment the loop counter and exit loop if counter == structure count */
+	add	w3, w3, #0x1
+	cmp	w1, w3
+	b.eq	2f
+
+	/* Read the 2nd word in header */
+	ldr	w2, [x0,#4]
+	/* Get the structure size from header */
+	ubfx	x2, x2, #SDS_HEADER_STRUCT_SIZE_SHIFT, #SDS_HEADER_STRUCT_SIZE_WIDTH
+	/* Add the structure size and SDS HEADER SIZE to point to next header */
+	add	x2, x2, #SDS_HEADER_SIZE
+	add	x0, x0, x2
+	b	loop_begin
+2:
+	mov	w0, #0xffffffff
+	ret
+endfunc sds_get_primary_cpu_id
diff --git a/drivers/arm/css/sds/sds.c b/drivers/arm/css/sds/sds.c
new file mode 100644
index 0000000..1fb196c
--- /dev/null
+++ b/drivers/arm/css/sds/sds.c
@@ -0,0 +1,259 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/css/sds.h>
+#include <platform_def.h>
+
+#include "sds_private.h"
+
+/*
+ * Variables used to track and maintain the state of the memory region reserved
+ * for usage by the SDS framework.
+ */
+
+/* Pointer to the base of the SDS memory region */
+static uintptr_t sds_mem_base;
+
+/* Size of the SDS memory region in bytes */
+static size_t sds_mem_size;
+
+/*
+ * Perform some non-exhaustive tests to determine whether any of the fields
+ * within a Structure Header contain obviously invalid data.
+ * Returns SDS_OK on success, SDS_ERR_FAIL on error.
+ */
+static int sds_struct_is_valid(uintptr_t header)
+{
+	size_t struct_size = GET_SDS_HEADER_STRUCT_SIZE(header);
+
+	/* Zero is not a valid identifier */
+	if (GET_SDS_HEADER_ID(header) == 0)
+		return SDS_ERR_FAIL;
+
+	/* Check SDS Schema version */
+	if (GET_SDS_HEADER_VERSION(header) == SDS_REGION_SCH_VERSION)
+		return SDS_ERR_FAIL;
+
+	/* The SDS Structure sizes have to be multiple of 8 */
+	if ((struct_size == 0) || ((struct_size % 8) != 0))
+		return SDS_ERR_FAIL;
+
+	if (struct_size > sds_mem_size)
+		return SDS_ERR_FAIL;
+
+	return SDS_OK;
+}
+
+/*
+ * Validate the SDS structure headers.
+ * Returns SDS_OK on success, SDS_ERR_FAIL on error.
+ */
+static int validate_sds_struct_headers(void)
+{
+	unsigned int i, structure_count;
+	uintptr_t header;
+
+	structure_count = GET_SDS_REGION_STRUCTURE_COUNT(sds_mem_base);
+
+	if (structure_count == 0)
+		return SDS_ERR_FAIL;
+
+	header = sds_mem_base + SDS_REGION_DESC_SIZE;
+
+	/* Iterate over structure headers and validate each one */
+	for (i = 0; i < structure_count; i++) {
+		if (sds_struct_is_valid(header) != SDS_OK) {
+			WARN("SDS: Invalid structure header detected\n");
+			return SDS_ERR_FAIL;
+		}
+		header += GET_SDS_HEADER_STRUCT_SIZE(header) + SDS_HEADER_SIZE;
+	}
+	return SDS_OK;
+}
+
+/*
+ * Get the structure header pointer corresponding to the structure ID.
+ * Returns SDS_OK on success, SDS_ERR_STRUCT_NOT_FOUND on error.
+ */
+static int get_struct_header(uint32_t structure_id, struct_header_t **header)
+{
+	unsigned int i, structure_count;
+	uintptr_t current_header;
+
+	assert(header);
+
+	structure_count = GET_SDS_REGION_STRUCTURE_COUNT(sds_mem_base);
+	if (structure_count == 0)
+		return SDS_ERR_STRUCT_NOT_FOUND;
+
+	current_header = ((uintptr_t)sds_mem_base) + SDS_REGION_DESC_SIZE;
+
+	/* Iterate over structure headers to find one with a matching ID */
+	for (i = 0; i < structure_count; i++) {
+		if (GET_SDS_HEADER_ID(current_header) == structure_id) {
+			*header = (struct_header_t *)current_header;
+			return SDS_OK;
+		}
+		current_header += GET_SDS_HEADER_STRUCT_SIZE(current_header) +
+						SDS_HEADER_SIZE;
+	}
+
+	*header = NULL;
+	return SDS_ERR_STRUCT_NOT_FOUND;
+}
+
+/*
+ * Check if a structure header corresponding to the structure ID exists.
+ * Returns SDS_OK if structure header exists else SDS_ERR_STRUCT_NOT_FOUND
+ * if not found.
+ */
+int sds_struct_exists(unsigned int structure_id)
+{
+	struct_header_t *header = NULL;
+	int ret;
+
+	ret = get_struct_header(structure_id, &header);
+	if (ret == SDS_OK) {
+		assert(header);
+	}
+
+	return ret;
+}
+
+/*
+ * Read from field in the structure corresponding to `structure_id`.
+ * `fld_off` is the offset to the field in the structure and `mode`
+ * indicates whether cache maintenance need to performed prior to the read.
+ * The `data` is the pointer to store the read data of size specified by `size`.
+ * Returns SDS_OK on success or corresponding error codes on failure.
+ */
+int sds_struct_read(uint32_t structure_id, unsigned int fld_off,
+		void *data, size_t size, sds_access_mode_t mode)
+{
+	int status;
+	uintptr_t field_base;
+	struct_header_t *header = NULL;
+
+	if (!data)
+		return SDS_ERR_INVALID_PARAMS;
+
+	/* Check if a structure with this ID exists */
+	status = get_struct_header(structure_id, &header);
+	if (status != SDS_OK)
+		return status;
+
+	assert(header);
+
+	if (mode == SDS_ACCESS_MODE_CACHED)
+		inv_dcache_range((uintptr_t)header, SDS_HEADER_SIZE + size);
+
+	if (!IS_SDS_HEADER_VALID(header)) {
+		WARN("SDS: Reading from un-finalized structure 0x%x\n",
+				structure_id);
+		return SDS_ERR_STRUCT_NOT_FINALIZED;
+	}
+
+	if ((fld_off + size) > GET_SDS_HEADER_STRUCT_SIZE(header))
+		return SDS_ERR_FAIL;
+
+	field_base = (uintptr_t)header + SDS_HEADER_SIZE + fld_off;
+	if (check_uptr_overflow(field_base, size - 1))
+		return SDS_ERR_FAIL;
+
+	/* Copy the required field in the struct */
+	memcpy(data, (void *)field_base, size);
+
+	return SDS_OK;
+}
+
+/*
+ * Write to the field in the structure corresponding to `structure_id`.
+ * `fld_off` is the offset to the field in the structure and `mode`
+ * indicates whether cache maintenance need to performed for the write.
+ * The `data` is the pointer to data of size specified by `size`.
+ * Returns SDS_OK on success or corresponding error codes on failure.
+ */
+int sds_struct_write(uint32_t structure_id, unsigned int fld_off,
+		void *data, size_t size, sds_access_mode_t mode)
+{
+	int status;
+	uintptr_t field_base;
+	struct_header_t *header = NULL;
+
+	if (!data)
+		return SDS_ERR_INVALID_PARAMS;
+
+	/* Check if a structure with this ID exists */
+	status = get_struct_header(structure_id, &header);
+	if (status != SDS_OK)
+		return status;
+
+	assert(header);
+
+	if (mode == SDS_ACCESS_MODE_CACHED)
+		inv_dcache_range((uintptr_t)header, SDS_HEADER_SIZE + size);
+
+	if (!IS_SDS_HEADER_VALID(header)) {
+		WARN("SDS: Writing to un-finalized structure 0x%x\n",
+				structure_id);
+		return SDS_ERR_STRUCT_NOT_FINALIZED;
+	}
+
+	if ((fld_off + size) > GET_SDS_HEADER_STRUCT_SIZE(header))
+		return SDS_ERR_FAIL;
+
+	field_base = (uintptr_t)header + SDS_HEADER_SIZE + fld_off;
+	if (check_uptr_overflow(field_base, size - 1))
+		return SDS_ERR_FAIL;
+
+	/* Copy the required field in the struct */
+	memcpy((void *)field_base, data, size);
+
+	if (mode == SDS_ACCESS_MODE_CACHED)
+		flush_dcache_range((uintptr_t)field_base, size);
+
+	return SDS_OK;
+}
+
+/*
+ * Initialize the SDS driver. Also verifies the SDS version and sanity of
+ * the SDS structure headers.
+ * Returns SDS_OK on success, SDS_ERR_FAIL on error.
+ */
+int sds_init(void)
+{
+	sds_mem_base = (uintptr_t)PLAT_ARM_SDS_MEM_BASE;
+
+	if (!IS_SDS_REGION_VALID(sds_mem_base)) {
+		WARN("SDS: No valid SDS Memory Region found\n");
+		return SDS_ERR_FAIL;
+	}
+
+	if (GET_SDS_REGION_SCHEMA_VERSION(sds_mem_base)
+				!= SDS_REGION_SCH_VERSION) {
+		WARN("SDS: Unsupported SDS schema version\n");
+		return SDS_ERR_FAIL;
+	}
+
+	sds_mem_size = GET_SDS_REGION_SIZE(sds_mem_base);
+	if (sds_mem_size > PLAT_ARM_SDS_MEM_SIZE_MAX) {
+		WARN("SDS: SDS Memory Region exceeds size limit\n");
+		return SDS_ERR_FAIL;
+	}
+
+	INFO("SDS: Detected SDS Memory Region (%zu bytes)\n", sds_mem_size);
+
+	if (validate_sds_struct_headers() != SDS_OK)
+		return SDS_ERR_FAIL;
+
+	return SDS_OK;
+}
diff --git a/drivers/arm/css/sds/sds_private.h b/drivers/arm/css/sds/sds_private.h
new file mode 100644
index 0000000..d801a04
--- /dev/null
+++ b/drivers/arm/css/sds/sds_private.h
@@ -0,0 +1,100 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef SDS_PRIVATE_H
+#define SDS_PRIVATE_H
+
+/* SDS Header defines */
+#define SDS_HEADER_ID_SHIFT			0
+#define SDS_HEADER_ID_WIDTH			16
+#define SDS_HEADER_ID_MASK			((1 << SDS_HEADER_ID_WIDTH) - 1)
+
+#define SDS_HEADER_MINOR_VERSION_WIDTH		8
+#define SDS_HEADER_MINOR_VERSION_SHIFT		16
+#define SDS_HEADER_MAJOR_VERSION_WIDTH		8
+
+#define MAKE_SDS_HEADER_VERSION(major, minor)	\
+	(((((major) & 0xff) << SDS_HEADER_MINOR_VERSION_WIDTH) | ((minor) & 0xff)))
+#define SDS_HEADER_VERSION_MASK			\
+	((1 << (SDS_HEADER_MINOR_VERSION_WIDTH + SDS_HEADER_MAJOR_VERSION_WIDTH)) - 1)
+
+#define SDS_HEADER_VERSION			MAKE_SDS_HEADER_VERSION(1, 0)
+#define SDS_HEADER_STRUCT_SIZE_WIDTH		23
+#define SDS_HEADER_STRUCT_SIZE_SHIFT		1
+#define SDS_HEADER_STRUCT_SIZE_MASK		((1 << SDS_HEADER_STRUCT_SIZE_WIDTH) - 1)
+#define SDS_HEADER_VALID_MASK			0x1
+#define SDS_HEADER_VALID_SHIFT			0
+#define SDS_HEADER_SIZE				0x8
+
+/* Arbitrary, 16 bit value that indicates a valid SDS Memory Region */
+#define SDS_REGION_SIGNATURE			0xAA7A
+#define SDS_REGION_SIGNATURE_WIDTH		16
+#define SDS_REGION_SIGNATURE_SHIFT		0
+#define SDS_REGION_SIGNATURE_MASK		((1 << SDS_REGION_SIGNATURE_WIDTH) - 1)
+
+#define SDS_REGION_STRUCT_COUNT_SHIFT		16
+#define SDS_REGION_STRUCT_COUNT_WIDTH		8
+#define SDS_REGION_STRUCT_COUNT_MASK		((1 << SDS_REGION_STRUCT_COUNT_WIDTH) - 1)
+
+#define SDS_REGION_SCH_MINOR_SHIFT		24
+#define SDS_REGION_SCH_MINOR_WIDTH		4
+#define SDS_REGION_SCH_MINOR_MASK		((1 << SDS_REGION_SCH_MINOR_WIDTH) - 1)
+
+#define SDS_REGION_SCH_MAJOR_SHIFT		28
+#define SDS_REGION_SCH_MAJOR_WIDTH		4
+#define SDS_REGION_SCH_MAJOR_MASK		((1 << SDS_REGION_SCH_MAJOR_WIDTH) - 1)
+
+#define SDS_REGION_SCH_VERSION_MASK		\
+	((1 << (SDS_REGION_SCH_MINOR_WIDTH + SDS_REGION_SCH_MAJOR_WIDTH)) - 1)
+
+#define MAKE_SDS_REGION_SCH_VERSION(maj, min)	\
+	((((maj) & SDS_REGION_SCH_MAJOR_MASK) << SDS_REGION_SCH_MINOR_WIDTH) |	\
+	((min) & SDS_REGION_SCH_MINOR_MASK))
+
+#define SDS_REGION_SCH_VERSION			MAKE_SDS_REGION_SCH_VERSION(1, 0)
+#define SDS_REGION_REGIONSIZE_OFFSET		0x4
+#define SDS_REGION_DESC_SIZE			0x8
+
+#ifndef __ASSEMBLER__
+#include <stddef.h>
+#include <stdint.h>
+
+/* Header containing Shared Data Structure metadata */
+typedef struct structure_header {
+	uint32_t reg[2];
+} struct_header_t;
+
+#define GET_SDS_HEADER_ID(_header)			\
+	((((struct_header_t *)(_header))->reg[0]) & SDS_HEADER_ID_MASK)
+#define GET_SDS_HEADER_VERSION(_header)			\
+	(((((struct_header_t *)(_header))->reg[0]) >> SDS_HEADER_MINOR_VERSION_SHIFT)\
+	& SDS_HEADER_VERSION_MASK)
+#define GET_SDS_HEADER_STRUCT_SIZE(_header)		\
+	(((((struct_header_t *)(_header))->reg[1]) >> SDS_HEADER_STRUCT_SIZE_SHIFT)\
+	& SDS_HEADER_STRUCT_SIZE_MASK)
+#define IS_SDS_HEADER_VALID(_header)			\
+	((((struct_header_t *)(_header))->reg[1]) & SDS_HEADER_VALID_MASK)
+#define GET_SDS_STRUCT_FIELD(_header, _field_offset)	\
+	((((uint8_t *)(_header)) + sizeof(struct_header_t)) + (_field_offset))
+
+/* Region Descriptor describing the SDS Memory Region */
+typedef struct region_descriptor {
+	uint32_t reg[2];
+} region_desc_t;
+
+#define IS_SDS_REGION_VALID(region)			\
+	(((((region_desc_t *)(region))->reg[0]) & SDS_REGION_SIGNATURE_MASK) == SDS_REGION_SIGNATURE)
+#define GET_SDS_REGION_STRUCTURE_COUNT(region)		\
+	(((((region_desc_t *)(region))->reg[0]) >> SDS_REGION_STRUCT_COUNT_SHIFT)\
+	& SDS_REGION_STRUCT_COUNT_MASK)
+#define GET_SDS_REGION_SCHEMA_VERSION(region)		\
+	(((((region_desc_t *)(region))->reg[0]) >> SDS_REGION_SCH_MINOR_SHIFT)\
+	& SDS_REGION_SCH_VERSION_MASK)
+#define GET_SDS_REGION_SIZE(region)		((((region_desc_t *)(region))->reg[1]))
+
+#endif /* __ASSEMBLER__ */
+
+#endif /* SDS_PRIVATE_H */
diff --git a/drivers/arm/dcc/dcc_console.c b/drivers/arm/dcc/dcc_console.c
new file mode 100644
index 0000000..0b7e541
--- /dev/null
+++ b/drivers/arm/dcc/dcc_console.c
@@ -0,0 +1,152 @@
+/*
+ * Copyright (c) 2015-2021, Xilinx Inc.
+ * Written by Michal Simek.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * Redistributions of source code must retain the above copyright notice, this
+ * list of conditions and the following disclaimer.
+ *
+ * Redistributions in binary form must reproduce the above copyright notice,
+ * this list of conditions and the following disclaimer in the documentation
+ * and/or other materials provided with the distribution.
+ *
+ * Neither the name of ARM nor the names of its contributors may be used
+ * to endorse or promote products derived from this software without specific
+ * prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#include <errno.h>
+#include <stddef.h>
+#include <arch_helpers.h>
+#include <drivers/arm/dcc.h>
+#include <drivers/console.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+/* DCC Status Bits */
+#define DCC_STATUS_RX		BIT(30)
+#define DCC_STATUS_TX		BIT(29)
+#define TIMEOUT_COUNT_US	U(0x10624)
+
+struct dcc_console {
+	struct console console;
+};
+
+static inline uint32_t __dcc_getstatus(void)
+{
+	return read_mdccsr_el0();
+}
+
+static inline char __dcc_getchar(void)
+{
+	char c;
+
+	c = read_dbgdtrrx_el0();
+
+	return c;
+}
+
+static inline void __dcc_putchar(char c)
+{
+	/*
+	 * The typecast is to make absolutely certain that 'c' is
+	 * zero-extended.
+	 */
+	write_dbgdtrtx_el0((unsigned char)c);
+}
+
+static int32_t dcc_status_timeout(uint32_t mask)
+{
+	const unsigned int timeout_count = TIMEOUT_COUNT_US;
+	uint64_t timeout;
+	unsigned int status;
+
+	timeout = timeout_init_us(timeout_count);
+
+	do {
+		status = (__dcc_getstatus() & mask);
+		if (timeout_elapsed(timeout)) {
+			return -ETIMEDOUT;
+		}
+	} while ((status != 0U));
+
+	return 0;
+}
+
+static int32_t dcc_console_putc(int32_t ch, struct console *console)
+{
+	unsigned int status;
+
+	status = dcc_status_timeout(DCC_STATUS_TX);
+	if (status != 0U) {
+		return status;
+	}
+	__dcc_putchar(ch);
+
+	return ch;
+}
+
+static int32_t dcc_console_getc(struct console *console)
+{
+	unsigned int status;
+
+	status = dcc_status_timeout(DCC_STATUS_RX);
+	if (status != 0U) {
+		return status;
+	}
+
+	return __dcc_getchar();
+}
+
+int32_t dcc_console_init(unsigned long base_addr, uint32_t uart_clk,
+		      uint32_t baud_rate)
+{
+	return 0; /* No init needed */
+}
+
+/**
+ * dcc_console_flush() - Function to force a write of all buffered data
+ *		          that hasn't been output.
+ * @console		Console struct
+ *
+ */
+static void dcc_console_flush(struct console *console)
+{
+	unsigned int status;
+
+	status = dcc_status_timeout(DCC_STATUS_TX);
+	if (status != 0U) {
+		return;
+	}
+}
+
+static struct dcc_console dcc_console = {
+	.console = {
+		.flags = CONSOLE_FLAG_BOOT |
+			CONSOLE_FLAG_RUNTIME,
+		.putc = dcc_console_putc,
+		.getc = dcc_console_getc,
+		.flush = dcc_console_flush,
+	},
+};
+
+int console_dcc_register(void)
+{
+	return console_register(&dcc_console.console);
+}
diff --git a/drivers/arm/ethosn/ethosn_smc.c b/drivers/arm/ethosn/ethosn_smc.c
new file mode 100644
index 0000000..915a0d8
--- /dev/null
+++ b/drivers/arm/ethosn/ethosn_smc.c
@@ -0,0 +1,229 @@
+/*
+ * Copyright (c) 2021-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <stdbool.h>
+
+#include <common/debug.h>
+#include <common/runtime_svc.h>
+#include <drivers/arm/ethosn.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+#include <plat/arm/common/fconf_ethosn_getter.h>
+
+/*
+ * Number of Arm(R) Ethos(TM)-N NPU (NPU) devices available
+ */
+#define ETHOSN_NUM_DEVICES \
+	FCONF_GET_PROPERTY(hw_config, ethosn_config, num_devices)
+
+#define ETHOSN_GET_DEVICE(dev_idx) \
+	FCONF_GET_PROPERTY(hw_config, ethosn_device, dev_idx)
+
+/* NPU core sec registry address */
+#define ETHOSN_CORE_SEC_REG(core_addr, reg_offset) \
+	(core_addr + reg_offset)
+
+/* Reset timeout in us */
+#define ETHOSN_RESET_TIMEOUT_US		U(10 * 1000 * 1000)
+#define ETHOSN_RESET_WAIT_US		U(1)
+
+#define SEC_DEL_REG			U(0x0004)
+#define SEC_DEL_VAL			U(0x81C)
+#define SEC_DEL_EXCC_MASK		U(0x20)
+
+#define SEC_SECCTLR_REG			U(0x0010)
+#define SEC_SECCTLR_VAL			U(0x3)
+
+#define SEC_DEL_ADDR_EXT_REG		U(0x201C)
+#define SEC_DEL_ADDR_EXT_VAL		U(0x15)
+
+#define SEC_SYSCTRL0_REG		U(0x0018)
+#define SEC_SYSCTRL0_SOFT_RESET		U(3U << 29)
+#define SEC_SYSCTRL0_HARD_RESET		U(1U << 31)
+
+#define SEC_MMUSID_REG_BASE		U(0x3008)
+#define SEC_MMUSID_OFFSET		U(0x1000)
+
+static bool ethosn_get_device_and_core(uintptr_t core_addr,
+				       const struct ethosn_device_t **dev_match,
+				       const struct ethosn_core_t **core_match)
+{
+	uint32_t dev_idx;
+	uint32_t core_idx;
+
+	for (dev_idx = 0U; dev_idx < ETHOSN_NUM_DEVICES; ++dev_idx) {
+		const struct ethosn_device_t *dev = ETHOSN_GET_DEVICE(dev_idx);
+
+		for (core_idx = 0U; core_idx < dev->num_cores; ++core_idx) {
+			const struct ethosn_core_t *core = &(dev->cores[core_idx]);
+
+			if (core->addr == core_addr) {
+				*dev_match = dev;
+				*core_match = core;
+				return true;
+			}
+		}
+	}
+
+	WARN("ETHOSN: Unknown core address given to SMC call.\n");
+	return false;
+}
+
+static void ethosn_configure_smmu_streams(const struct ethosn_device_t *device,
+					  const struct ethosn_core_t *core,
+					  uint32_t asset_alloc_idx)
+{
+	const struct ethosn_main_allocator_t *main_alloc =
+		&(core->main_allocator);
+	const struct ethosn_asset_allocator_t *asset_alloc =
+		&(device->asset_allocators[asset_alloc_idx]);
+	const uint32_t streams[9] = {
+		main_alloc->firmware.stream_id,
+		main_alloc->working_data.stream_id,
+		asset_alloc->command_stream.stream_id,
+		0U, /* Not used*/
+		main_alloc->firmware.stream_id,
+		asset_alloc->weight_data.stream_id,
+		asset_alloc->buffer_data.stream_id,
+		asset_alloc->intermediate_data.stream_id,
+		asset_alloc->buffer_data.stream_id
+	};
+	size_t i;
+
+	for (i = 0U; i < ARRAY_SIZE(streams); ++i) {
+		const uintptr_t reg_addr = SEC_MMUSID_REG_BASE +
+			(SEC_MMUSID_OFFSET * i);
+		mmio_write_32(ETHOSN_CORE_SEC_REG(core->addr, reg_addr),
+			      streams[i]);
+	}
+}
+
+static void ethosn_delegate_to_ns(uintptr_t core_addr)
+{
+	mmio_setbits_32(ETHOSN_CORE_SEC_REG(core_addr, SEC_SECCTLR_REG),
+			SEC_SECCTLR_VAL);
+
+	mmio_setbits_32(ETHOSN_CORE_SEC_REG(core_addr, SEC_DEL_REG),
+			SEC_DEL_VAL);
+
+	mmio_setbits_32(ETHOSN_CORE_SEC_REG(core_addr, SEC_DEL_ADDR_EXT_REG),
+			SEC_DEL_ADDR_EXT_VAL);
+}
+
+static int ethosn_is_sec(uintptr_t core_addr)
+{
+	if ((mmio_read_32(ETHOSN_CORE_SEC_REG(core_addr, SEC_DEL_REG))
+		& SEC_DEL_EXCC_MASK) != 0U) {
+		return 0;
+	}
+
+	return 1;
+}
+
+static bool ethosn_reset(uintptr_t core_addr, int hard_reset)
+{
+	unsigned int timeout;
+	const uintptr_t sysctrl0_reg =
+		ETHOSN_CORE_SEC_REG(core_addr, SEC_SYSCTRL0_REG);
+	const uint32_t reset_val = (hard_reset != 0) ? SEC_SYSCTRL0_HARD_RESET
+						    : SEC_SYSCTRL0_SOFT_RESET;
+
+	mmio_write_32(sysctrl0_reg, reset_val);
+
+	/* Wait for reset to complete */
+	for (timeout = 0U; timeout < ETHOSN_RESET_TIMEOUT_US;
+			   timeout += ETHOSN_RESET_WAIT_US) {
+
+		if ((mmio_read_32(sysctrl0_reg) & reset_val) == 0U) {
+			break;
+		}
+
+		udelay(ETHOSN_RESET_WAIT_US);
+	}
+
+	return timeout < ETHOSN_RESET_TIMEOUT_US;
+}
+
+uintptr_t ethosn_smc_handler(uint32_t smc_fid,
+			     u_register_t core_addr,
+			     u_register_t asset_alloc_idx,
+			     u_register_t x3,
+			     u_register_t x4,
+			     void *cookie,
+			     void *handle,
+			     u_register_t flags)
+{
+	int hard_reset = 0;
+	const struct ethosn_device_t *device = NULL;
+	const struct ethosn_core_t *core = NULL;
+	const uint32_t fid = smc_fid & FUNCID_NUM_MASK;
+
+	/* Only SiP fast calls are expected */
+	if ((GET_SMC_TYPE(smc_fid) != SMC_TYPE_FAST) ||
+		(GET_SMC_OEN(smc_fid) != OEN_SIP_START)) {
+		SMC_RET1(handle, SMC_UNK);
+	}
+
+	/* Truncate parameters to 32-bits for SMC32 */
+	if (GET_SMC_CC(smc_fid) == SMC_32) {
+		core_addr &= 0xFFFFFFFF;
+		asset_alloc_idx &= 0xFFFFFFFF;
+		x3 &= 0xFFFFFFFF;
+		x4 &= 0xFFFFFFFF;
+	}
+
+	if (!is_ethosn_fid(smc_fid) ||
+	    (fid < ETHOSN_FNUM_VERSION || fid > ETHOSN_FNUM_SOFT_RESET)) {
+		WARN("ETHOSN: Unknown SMC call: 0x%x\n", smc_fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+
+	/* Commands that do not require a valid core address */
+	switch (fid) {
+	case ETHOSN_FNUM_VERSION:
+		SMC_RET2(handle, ETHOSN_VERSION_MAJOR, ETHOSN_VERSION_MINOR);
+	}
+
+	if (!ethosn_get_device_and_core(core_addr, &device, &core))  {
+		SMC_RET1(handle, ETHOSN_UNKNOWN_CORE_ADDRESS);
+	}
+
+	/* Commands that require a valid core address */
+	switch (fid) {
+	case ETHOSN_FNUM_IS_SEC:
+		SMC_RET1(handle, ethosn_is_sec(core->addr));
+	}
+
+	if (!device->has_reserved_memory &&
+	    asset_alloc_idx >= device->num_allocators) {
+		WARN("ETHOSN: Unknown asset allocator index given to SMC call.\n");
+		SMC_RET1(handle, ETHOSN_UNKNOWN_ALLOCATOR_IDX);
+	}
+
+	/* Commands that require a valid device, core and asset allocator */
+	switch (fid) {
+	case ETHOSN_FNUM_HARD_RESET:
+		hard_reset = 1;
+		/* Fallthrough */
+	case ETHOSN_FNUM_SOFT_RESET:
+		if (!ethosn_reset(core->addr, hard_reset)) {
+			SMC_RET1(handle, ETHOSN_FAILURE);
+		}
+
+		if (!device->has_reserved_memory) {
+			ethosn_configure_smmu_streams(device, core,
+						      asset_alloc_idx);
+		}
+
+		ethosn_delegate_to_ns(core->addr);
+		SMC_RET1(handle, ETHOSN_SUCCESS);
+	default:
+		WARN("ETHOSN: Unimplemented SMC call: 0x%x\n", fid);
+		SMC_RET1(handle, SMC_UNK);
+	}
+}
diff --git a/drivers/arm/fvp/fvp_pwrc.c b/drivers/arm/fvp/fvp_pwrc.c
new file mode 100644
index 0000000..75a2b66
--- /dev/null
+++ b/drivers/arm/fvp/fvp_pwrc.c
@@ -0,0 +1,78 @@
+/*
+ * Copyright (c) 2013-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <drivers/arm/fvp/fvp_pwrc.h>
+#include <lib/bakery_lock.h>
+#include <lib/mmio.h>
+#include <plat/arm/common/plat_arm.h>
+#include <platform_def.h>
+
+/*
+ * TODO: Someday there will be a generic power controller api. At the moment
+ * each platform has its own pwrc so just exporting functions is fine.
+ */
+ARM_INSTANTIATE_LOCK;
+
+unsigned int fvp_pwrc_get_cpu_wkr(u_register_t mpidr)
+{
+	return PSYSR_WK(fvp_pwrc_read_psysr(mpidr));
+}
+
+unsigned int fvp_pwrc_read_psysr(u_register_t mpidr)
+{
+	unsigned int rc;
+	arm_lock_get();
+	mmio_write_32(PWRC_BASE + PSYSR_OFF, (unsigned int) mpidr);
+	rc = mmio_read_32(PWRC_BASE + PSYSR_OFF);
+	arm_lock_release();
+	return rc;
+}
+
+void fvp_pwrc_write_pponr(u_register_t mpidr)
+{
+	arm_lock_get();
+	mmio_write_32(PWRC_BASE + PPONR_OFF, (unsigned int) mpidr);
+	arm_lock_release();
+}
+
+void fvp_pwrc_write_ppoffr(u_register_t mpidr)
+{
+	arm_lock_get();
+	mmio_write_32(PWRC_BASE + PPOFFR_OFF, (unsigned int) mpidr);
+	arm_lock_release();
+}
+
+void fvp_pwrc_set_wen(u_register_t mpidr)
+{
+	arm_lock_get();
+	mmio_write_32(PWRC_BASE + PWKUPR_OFF,
+		      (unsigned int) (PWKUPR_WEN | mpidr));
+	arm_lock_release();
+}
+
+void fvp_pwrc_clr_wen(u_register_t mpidr)
+{
+	arm_lock_get();
+	mmio_write_32(PWRC_BASE + PWKUPR_OFF,
+		      (unsigned int) mpidr);
+	arm_lock_release();
+}
+
+void fvp_pwrc_write_pcoffr(u_register_t mpidr)
+{
+	arm_lock_get();
+	mmio_write_32(PWRC_BASE + PCOFFR_OFF, (unsigned int) mpidr);
+	arm_lock_release();
+}
+
+/* Nothing else to do here apart from initializing the lock */
+void __init plat_arm_pwrc_setup(void)
+{
+	arm_lock_init();
+}
+
+
+
diff --git a/drivers/arm/gic/common/gic_common.c b/drivers/arm/gic/common/gic_common.c
new file mode 100644
index 0000000..bf6405f
--- /dev/null
+++ b/drivers/arm/gic/common/gic_common.c
@@ -0,0 +1,342 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#pragma message __FILE__ " is deprecated, use gicv2.mk instead"
+
+#include <assert.h>
+
+#include <drivers/arm/gic_common.h>
+#include <lib/mmio.h>
+
+#include "gic_common_private.h"
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading entire registers
+ ******************************************************************************/
+/*
+ * Accessor to read the GIC Distributor IGROUPR corresponding to the interrupt
+ * `id`, 32 interrupt ids at a time.
+ */
+unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> IGROUPR_SHIFT;
+
+	return mmio_read_32(base + GICD_IGROUPR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISENABLER corresponding to the
+ * interrupt `id`, 32 interrupt ids at a time.
+ */
+unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISENABLER_SHIFT;
+
+	return mmio_read_32(base + GICD_ISENABLER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICENABLER_SHIFT;
+
+	return mmio_read_32(base + GICD_ICENABLER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISPENDR_SHIFT;
+
+	return mmio_read_32(base + GICD_ISPENDR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICPENDR_SHIFT;
+
+	return mmio_read_32(base + GICD_ICPENDR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISACTIVER_SHIFT;
+
+	return mmio_read_32(base + GICD_ISACTIVER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICACTIVER_SHIFT;
+
+	return mmio_read_32(base + GICD_ICACTIVER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor IPRIORITYR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> IPRIORITYR_SHIFT;
+
+	return mmio_read_32(base + GICD_IPRIORITYR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICGFR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICFGR_SHIFT;
+
+	return mmio_read_32(base + GICD_ICFGR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor NSACR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> NSACR_SHIFT;
+
+	return mmio_read_32(base + GICD_NSACR + (n << 2));
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for writing entire registers
+ ******************************************************************************/
+/*
+ * Accessor to write the GIC Distributor IGROUPR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> IGROUPR_SHIFT;
+
+	mmio_write_32(base + GICD_IGROUPR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISENABLER_SHIFT;
+
+	mmio_write_32(base + GICD_ISENABLER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICENABLER_SHIFT;
+
+	mmio_write_32(base + GICD_ICENABLER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISPENDR_SHIFT;
+
+	mmio_write_32(base + GICD_ISPENDR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICPENDR_SHIFT;
+
+	mmio_write_32(base + GICD_ICPENDR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISACTIVER_SHIFT;
+
+	mmio_write_32(base + GICD_ISACTIVER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICACTIVER_SHIFT;
+
+	mmio_write_32(base + GICD_ICACTIVER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor IPRIORITYR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> IPRIORITYR_SHIFT;
+
+	mmio_write_32(base + GICD_IPRIORITYR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICFGR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICFGR_SHIFT;
+
+	mmio_write_32(base + GICD_ICFGR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor NSACR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> NSACR_SHIFT;
+
+	mmio_write_32(base + GICD_NSACR + (n << 2), val);
+}
+
+/*******************************************************************************
+ * GIC Distributor functions for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	return (reg_val >> bit_num) & 0x1U;
+}
+
+void gicd_set_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	gicd_write_igroupr(base, id, reg_val | (1U << bit_num));
+}
+
+void gicd_clr_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	gicd_write_igroupr(base, id, reg_val & ~(1U << bit_num));
+}
+
+void gicd_set_isenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISENABLER_SHIFT) - 1U);
+
+	gicd_write_isenabler(base, id, (1U << bit_num));
+}
+
+void gicd_set_icenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICENABLER_SHIFT) - 1U);
+
+	gicd_write_icenabler(base, id, (1U << bit_num));
+}
+
+void gicd_set_ispendr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISPENDR_SHIFT) - 1U);
+
+	gicd_write_ispendr(base, id, (1U << bit_num));
+}
+
+void gicd_set_icpendr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICPENDR_SHIFT) - 1U);
+
+	gicd_write_icpendr(base, id, (1U << bit_num));
+}
+
+unsigned int gicd_get_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISACTIVER_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_isactiver(base, id);
+
+	return (reg_val >> bit_num) & 0x1U;
+}
+
+void gicd_set_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISACTIVER_SHIFT) - 1U);
+
+	gicd_write_isactiver(base, id, (1U << bit_num));
+}
+
+void gicd_set_icactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICACTIVER_SHIFT) - 1U);
+
+	gicd_write_icactiver(base, id, (1U << bit_num));
+}
+
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	uint8_t val = pri & GIC_PRI_MASK;
+
+	mmio_write_8(base + GICD_IPRIORITYR + id, val);
+}
+
+void gicd_set_icfgr(uintptr_t base, unsigned int id, unsigned int cfg)
+{
+	/* Interrupt configuration is a 2-bit field */
+	unsigned int bit_num = id & ((1U << ICFGR_SHIFT) - 1U);
+	unsigned int bit_shift = bit_num << 1;
+
+	uint32_t reg_val = gicd_read_icfgr(base, id);
+
+	/* Clear the field, and insert required configuration */
+	reg_val &= ~(GIC_CFG_MASK << bit_shift);
+	reg_val |= ((cfg & GIC_CFG_MASK) << bit_shift);
+
+	gicd_write_icfgr(base, id, reg_val);
+}
diff --git a/drivers/arm/gic/common/gic_common_private.h b/drivers/arm/gic/common/gic_common_private.h
new file mode 100644
index 0000000..1ab1bdb
--- /dev/null
+++ b/drivers/arm/gic/common/gic_common_private.h
@@ -0,0 +1,89 @@
+/*
+ * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef GIC_COMMON_PRIVATE_H
+#define GIC_COMMON_PRIVATE_H
+
+#include <stdint.h>
+
+#include <drivers/arm/gic_common.h>
+#include <lib/mmio.h>
+
+/*******************************************************************************
+ * GIC Distributor interface register accessors that are common to GICv3 & GICv2
+ ******************************************************************************/
+static inline unsigned int gicd_read_ctlr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_CTLR);
+}
+
+static inline unsigned int gicd_read_typer(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_TYPER);
+}
+
+static inline unsigned int gicd_read_iidr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_IIDR);
+}
+
+static inline void gicd_write_ctlr(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICD_CTLR, val);
+}
+
+/*******************************************************************************
+ * GIC Distributor function prototypes for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
+ ******************************************************************************/
+unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id);
+unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id);
+unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id);
+unsigned int gicd_read_spendsgir(uintptr_t base, unsigned int id);
+unsigned int gicd_read_cpendsgir(uintptr_t base, unsigned int id);
+unsigned int gicd_read_itargetsr(uintptr_t base, unsigned int id);
+void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_spendsgir(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_cpendsgir(uintptr_t base, unsigned int id, unsigned int val);
+void gicd_write_itargetsr(uintptr_t base, unsigned int id, unsigned int val);
+
+/*******************************************************************************
+ * GIC Distributor function prototypes for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id);
+void gicd_set_igroupr(uintptr_t base, unsigned int id);
+void gicd_clr_igroupr(uintptr_t base, unsigned int id);
+void gicd_set_isenabler(uintptr_t base, unsigned int id);
+void gicd_set_icenabler(uintptr_t base, unsigned int id);
+void gicd_set_ispendr(uintptr_t base, unsigned int id);
+void gicd_set_icpendr(uintptr_t base, unsigned int id);
+unsigned int gicd_get_isactiver(uintptr_t base, unsigned int id);
+void gicd_set_isactiver(uintptr_t base, unsigned int id);
+void gicd_set_icactiver(uintptr_t base, unsigned int id);
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri);
+void gicd_set_icfgr(uintptr_t base, unsigned int id, unsigned int cfg);
+
+#endif /* GIC_COMMON_PRIVATE_H */
diff --git a/drivers/arm/gic/v2/gicdv2_helpers.c b/drivers/arm/gic/v2/gicdv2_helpers.c
new file mode 100644
index 0000000..db9ba87
--- /dev/null
+++ b/drivers/arm/gic/v2/gicdv2_helpers.c
@@ -0,0 +1,340 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <drivers/arm/gic_common.h>
+#include <lib/mmio.h>
+
+#include "../common/gic_common_private.h"
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading entire registers
+ ******************************************************************************/
+/*
+ * Accessor to read the GIC Distributor IGROUPR corresponding to the interrupt
+ * `id`, 32 interrupt ids at a time.
+ */
+unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> IGROUPR_SHIFT;
+
+	return mmio_read_32(base + GICD_IGROUPR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISENABLER corresponding to the
+ * interrupt `id`, 32 interrupt ids at a time.
+ */
+unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISENABLER_SHIFT;
+
+	return mmio_read_32(base + GICD_ISENABLER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICENABLER_SHIFT;
+
+	return mmio_read_32(base + GICD_ICENABLER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISPENDR_SHIFT;
+
+	return mmio_read_32(base + GICD_ISPENDR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icpendr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICPENDR_SHIFT;
+
+	return mmio_read_32(base + GICD_ICPENDR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ISACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ISACTIVER_SHIFT;
+
+	return mmio_read_32(base + GICD_ISACTIVER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICACTIVER_SHIFT;
+
+	return mmio_read_32(base + GICD_ICACTIVER + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor IPRIORITYR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> IPRIORITYR_SHIFT;
+
+	return mmio_read_32(base + GICD_IPRIORITYR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor ICGFR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> ICFGR_SHIFT;
+
+	return mmio_read_32(base + GICD_ICFGR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor NSACR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id)
+{
+	unsigned int n = id >> NSACR_SHIFT;
+
+	return mmio_read_32(base + GICD_NSACR + (n << 2));
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for writing entire registers
+ ******************************************************************************/
+/*
+ * Accessor to write the GIC Distributor IGROUPR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> IGROUPR_SHIFT;
+
+	mmio_write_32(base + GICD_IGROUPR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISENABLER_SHIFT;
+
+	mmio_write_32(base + GICD_ISENABLER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICENABLER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icenabler(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICENABLER_SHIFT;
+
+	mmio_write_32(base + GICD_ICENABLER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISPENDR_SHIFT;
+
+	mmio_write_32(base + GICD_ISPENDR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICPENDR corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icpendr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICPENDR_SHIFT;
+
+	mmio_write_32(base + GICD_ICPENDR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ISACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ISACTIVER_SHIFT;
+
+	mmio_write_32(base + GICD_ISACTIVER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICACTIVER corresponding to the
+ * interrupt `id`, 32 interrupt IDs at a time.
+ */
+void gicd_write_icactiver(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICACTIVER_SHIFT;
+
+	mmio_write_32(base + GICD_ICACTIVER + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor IPRIORITYR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> IPRIORITYR_SHIFT;
+
+	mmio_write_32(base + GICD_IPRIORITYR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor ICFGR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> ICFGR_SHIFT;
+
+	mmio_write_32(base + GICD_ICFGR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor NSACR corresponding to the
+ * interrupt `id`, 16 interrupt IDs at a time.
+ */
+void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned int n = id >> NSACR_SHIFT;
+
+	mmio_write_32(base + GICD_NSACR + (n << 2), val);
+}
+
+/*******************************************************************************
+ * GIC Distributor functions for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	return (reg_val >> bit_num) & 0x1U;
+}
+
+void gicd_set_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	gicd_write_igroupr(base, id, reg_val | (1U << bit_num));
+}
+
+void gicd_clr_igroupr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << IGROUPR_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_igroupr(base, id);
+
+	gicd_write_igroupr(base, id, reg_val & ~(1U << bit_num));
+}
+
+void gicd_set_isenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISENABLER_SHIFT) - 1U);
+
+	gicd_write_isenabler(base, id, (1U << bit_num));
+}
+
+void gicd_set_icenabler(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICENABLER_SHIFT) - 1U);
+
+	gicd_write_icenabler(base, id, (1U << bit_num));
+}
+
+void gicd_set_ispendr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISPENDR_SHIFT) - 1U);
+
+	gicd_write_ispendr(base, id, (1U << bit_num));
+}
+
+void gicd_set_icpendr(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICPENDR_SHIFT) - 1U);
+
+	gicd_write_icpendr(base, id, (1U << bit_num));
+}
+
+unsigned int gicd_get_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISACTIVER_SHIFT) - 1U);
+	unsigned int reg_val = gicd_read_isactiver(base, id);
+
+	return (reg_val >> bit_num) & 0x1U;
+}
+
+void gicd_set_isactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ISACTIVER_SHIFT) - 1U);
+
+	gicd_write_isactiver(base, id, (1U << bit_num));
+}
+
+void gicd_set_icactiver(uintptr_t base, unsigned int id)
+{
+	unsigned int bit_num = id & ((1U << ICACTIVER_SHIFT) - 1U);
+
+	gicd_write_icactiver(base, id, (1U << bit_num));
+}
+
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	uint8_t val = pri & GIC_PRI_MASK;
+
+	mmio_write_8(base + GICD_IPRIORITYR + id, val);
+}
+
+void gicd_set_icfgr(uintptr_t base, unsigned int id, unsigned int cfg)
+{
+	/* Interrupt configuration is a 2-bit field */
+	unsigned int bit_num = id & ((1U << ICFGR_SHIFT) - 1U);
+	unsigned int bit_shift = bit_num << 1;
+
+	uint32_t reg_val = gicd_read_icfgr(base, id);
+
+	/* Clear the field, and insert required configuration */
+	reg_val &= ~(GIC_CFG_MASK << bit_shift);
+	reg_val |= ((cfg & GIC_CFG_MASK) << bit_shift);
+
+	gicd_write_icfgr(base, id, reg_val);
+}
diff --git a/drivers/arm/gic/v2/gicv2.mk b/drivers/arm/gic/v2/gicv2.mk
new file mode 100644
index 0000000..49996bb
--- /dev/null
+++ b/drivers/arm/gic/v2/gicv2.mk
@@ -0,0 +1,15 @@
+#
+# Copyright (c) 2020, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# No support for extended PPI and SPI range
+GIC_EXT_INTID	:=	0
+
+GICV2_SOURCES	+=	drivers/arm/gic/v2/gicv2_main.c		\
+			drivers/arm/gic/v2/gicv2_helpers.c	\
+			drivers/arm/gic/v2/gicdv2_helpers.c
+
+# Set GICv2 build option
+$(eval $(call add_define,GIC_EXT_INTID))
\ No newline at end of file
diff --git a/drivers/arm/gic/v2/gicv2_helpers.c b/drivers/arm/gic/v2/gicv2_helpers.c
new file mode 100644
index 0000000..751316c
--- /dev/null
+++ b/drivers/arm/gic/v2/gicv2_helpers.c
@@ -0,0 +1,220 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch.h>
+#include <common/debug.h>
+#include <common/interrupt_props.h>
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv2.h>
+
+#include "../common/gic_common_private.h"
+#include "gicv2_private.h"
+
+/*
+ * Accessor to read the GIC Distributor ITARGETSR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+unsigned int gicd_read_itargetsr(uintptr_t base, unsigned int id)
+{
+	unsigned n = id >> ITARGETSR_SHIFT;
+	return mmio_read_32(base + GICD_ITARGETSR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor CPENDSGIR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+unsigned int gicd_read_cpendsgir(uintptr_t base, unsigned int id)
+{
+	unsigned n = id >> CPENDSGIR_SHIFT;
+	return mmio_read_32(base + GICD_CPENDSGIR + (n << 2));
+}
+
+/*
+ * Accessor to read the GIC Distributor SPENDSGIR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+unsigned int gicd_read_spendsgir(uintptr_t base, unsigned int id)
+{
+	unsigned n = id >> SPENDSGIR_SHIFT;
+	return mmio_read_32(base + GICD_SPENDSGIR + (n << 2));
+}
+
+/*
+ * Accessor to write the GIC Distributor ITARGETSR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+void gicd_write_itargetsr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned n = id >> ITARGETSR_SHIFT;
+	mmio_write_32(base + GICD_ITARGETSR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor CPENDSGIR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+void gicd_write_cpendsgir(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned n = id >> CPENDSGIR_SHIFT;
+	mmio_write_32(base + GICD_CPENDSGIR + (n << 2), val);
+}
+
+/*
+ * Accessor to write the GIC Distributor SPENDSGIR corresponding to the
+ * interrupt `id`, 4 interrupt IDs at a time.
+ */
+void gicd_write_spendsgir(uintptr_t base, unsigned int id, unsigned int val)
+{
+	unsigned n = id >> SPENDSGIR_SHIFT;
+	mmio_write_32(base + GICD_SPENDSGIR + (n << 2), val);
+}
+
+/*******************************************************************************
+ * Get the current CPU bit mask from GICD_ITARGETSR0
+ ******************************************************************************/
+unsigned int gicv2_get_cpuif_id(uintptr_t base)
+{
+	unsigned int val;
+
+	val = gicd_read_itargetsr(base, 0);
+	return val & GIC_TARGET_CPU_MASK;
+}
+
+/*******************************************************************************
+ * Helper function to configure the default attributes of SPIs.
+ ******************************************************************************/
+void gicv2_spis_configure_defaults(uintptr_t gicd_base)
+{
+	unsigned int index, num_ints;
+
+	num_ints = gicd_read_typer(gicd_base);
+	num_ints &= TYPER_IT_LINES_NO_MASK;
+	num_ints = (num_ints + 1U) << 5;
+
+	/*
+	 * Treat all SPIs as G1NS by default. The number of interrupts is
+	 * calculated as 32 * (IT_LINES + 1). We do 32 at a time.
+	 */
+	for (index = MIN_SPI_ID; index < num_ints; index += 32U)
+		gicd_write_igroupr(gicd_base, index, ~0U);
+
+	/* Setup the default SPI priorities doing four at a time */
+	for (index = MIN_SPI_ID; index < num_ints; index += 4U)
+		gicd_write_ipriorityr(gicd_base,
+				      index,
+				      GICD_IPRIORITYR_DEF_VAL);
+
+	/* Treat all SPIs as level triggered by default, 16 at a time */
+	for (index = MIN_SPI_ID; index < num_ints; index += 16U)
+		gicd_write_icfgr(gicd_base, index, 0U);
+}
+
+/*******************************************************************************
+ * Helper function to configure properties of secure G0 SPIs.
+ ******************************************************************************/
+void gicv2_secure_spis_configure_props(uintptr_t gicd_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num)
+{
+	unsigned int i;
+	const interrupt_prop_t *prop_desc;
+
+	/* Make sure there's a valid property array */
+	if (interrupt_props_num != 0U)
+		assert(interrupt_props != NULL);
+
+	for (i = 0; i < interrupt_props_num; i++) {
+		prop_desc = &interrupt_props[i];
+
+		if (prop_desc->intr_num < MIN_SPI_ID)
+			continue;
+
+		/* Configure this interrupt as a secure interrupt */
+		assert(prop_desc->intr_grp == GICV2_INTR_GROUP0);
+		gicd_clr_igroupr(gicd_base, prop_desc->intr_num);
+
+		/* Set the priority of this interrupt */
+		gicd_set_ipriorityr(gicd_base, prop_desc->intr_num,
+				prop_desc->intr_pri);
+
+		/* Target the secure interrupts to primary CPU */
+		gicd_set_itargetsr(gicd_base, prop_desc->intr_num,
+				gicv2_get_cpuif_id(gicd_base));
+
+		/* Set interrupt configuration */
+		gicd_set_icfgr(gicd_base, prop_desc->intr_num,
+				prop_desc->intr_cfg);
+
+		/* Enable this interrupt */
+		gicd_set_isenabler(gicd_base, prop_desc->intr_num);
+	}
+}
+
+/*******************************************************************************
+ * Helper function to configure properties of secure G0 SGIs and PPIs.
+ ******************************************************************************/
+void gicv2_secure_ppi_sgi_setup_props(uintptr_t gicd_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num)
+{
+	unsigned int i;
+	uint32_t sec_ppi_sgi_mask = 0;
+	const interrupt_prop_t *prop_desc;
+
+	/* Make sure there's a valid property array */
+	if (interrupt_props_num != 0U)
+		assert(interrupt_props != NULL);
+
+	/*
+	 * Disable all SGIs (imp. def.)/PPIs before configuring them. This is a
+	 * more scalable approach as it avoids clearing the enable bits in the
+	 * GICD_CTLR.
+	 */
+	gicd_write_icenabler(gicd_base, 0U, ~0U);
+
+	/* Setup the default PPI/SGI priorities doing four at a time */
+	for (i = 0U; i < MIN_SPI_ID; i += 4U)
+		gicd_write_ipriorityr(gicd_base, i, GICD_IPRIORITYR_DEF_VAL);
+
+	for (i = 0U; i < interrupt_props_num; i++) {
+		prop_desc = &interrupt_props[i];
+
+		if (prop_desc->intr_num >= MIN_SPI_ID)
+			continue;
+
+		/* Configure this interrupt as a secure interrupt */
+		assert(prop_desc->intr_grp == GICV2_INTR_GROUP0);
+
+		/*
+		 * Set interrupt configuration for PPIs. Configuration for SGIs
+		 * are ignored.
+		 */
+		if ((prop_desc->intr_num >= MIN_PPI_ID) &&
+				(prop_desc->intr_num < MIN_SPI_ID)) {
+			gicd_set_icfgr(gicd_base, prop_desc->intr_num,
+					prop_desc->intr_cfg);
+		}
+
+		/* We have an SGI or a PPI. They are Group0 at reset */
+		sec_ppi_sgi_mask |= (1u << prop_desc->intr_num);
+
+		/* Set the priority of this interrupt */
+		gicd_set_ipriorityr(gicd_base, prop_desc->intr_num,
+				prop_desc->intr_pri);
+	}
+
+	/*
+	 * Invert the bitmask to create a mask for non-secure PPIs and SGIs.
+	 * Program the GICD_IGROUPR0 with this bit mask.
+	 */
+	gicd_write_igroupr(gicd_base, 0, ~sec_ppi_sgi_mask);
+
+	/* Enable the Group 0 SGIs and PPIs */
+	gicd_write_isenabler(gicd_base, 0, sec_ppi_sgi_mask);
+}
diff --git a/drivers/arm/gic/v2/gicv2_main.c b/drivers/arm/gic/v2/gicv2_main.c
new file mode 100644
index 0000000..1925a13
--- /dev/null
+++ b/drivers/arm/gic/v2/gicv2_main.c
@@ -0,0 +1,556 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ * Portions copyright (c) 2021-2022, ProvenRun S.A.S. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/interrupt_props.h>
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv2.h>
+#include <lib/spinlock.h>
+
+#include "../common/gic_common_private.h"
+#include "gicv2_private.h"
+
+static const gicv2_driver_data_t *driver_data;
+
+/*
+ * Spinlock to guard registers needing read-modify-write. APIs protected by this
+ * spinlock are used either at boot time (when only a single CPU is active), or
+ * when the system is fully coherent.
+ */
+static spinlock_t gic_lock;
+
+/*******************************************************************************
+ * Enable secure interrupts and use FIQs to route them. Disable legacy bypass
+ * and set the priority mask register to allow all interrupts to trickle in.
+ ******************************************************************************/
+void gicv2_cpuif_enable(void)
+{
+	unsigned int val;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	/*
+	 * Enable the Group 0 interrupts, FIQEn and disable Group 0/1
+	 * bypass.
+	 */
+	val = CTLR_ENABLE_G0_BIT | FIQ_EN_BIT | FIQ_BYP_DIS_GRP0;
+	val |= IRQ_BYP_DIS_GRP0 | FIQ_BYP_DIS_GRP1 | IRQ_BYP_DIS_GRP1;
+
+	/* Program the idle priority in the PMR */
+	gicc_write_pmr(driver_data->gicc_base, GIC_PRI_MASK);
+	gicc_write_ctlr(driver_data->gicc_base, val);
+}
+
+/*******************************************************************************
+ * Place the cpu interface in a state where it can never make a cpu exit wfi as
+ * as result of an asserted interrupt. This is critical for powering down a cpu
+ ******************************************************************************/
+void gicv2_cpuif_disable(void)
+{
+	unsigned int val;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	/* Disable secure, non-secure interrupts and disable their bypass */
+	val = gicc_read_ctlr(driver_data->gicc_base);
+	val &= ~(CTLR_ENABLE_G0_BIT | CTLR_ENABLE_G1_BIT);
+	val |= FIQ_BYP_DIS_GRP1 | FIQ_BYP_DIS_GRP0;
+	val |= IRQ_BYP_DIS_GRP0 | IRQ_BYP_DIS_GRP1;
+	gicc_write_ctlr(driver_data->gicc_base, val);
+}
+
+/*******************************************************************************
+ * Per cpu gic distributor setup which will be done by all cpus after a cold
+ * boot/hotplug. This marks out the secure SPIs and PPIs & enables them.
+ ******************************************************************************/
+void gicv2_pcpu_distif_init(void)
+{
+	unsigned int ctlr;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+
+	gicv2_secure_ppi_sgi_setup_props(driver_data->gicd_base,
+			driver_data->interrupt_props,
+			driver_data->interrupt_props_num);
+
+	/* Enable G0 interrupts if not already */
+	ctlr = gicd_read_ctlr(driver_data->gicd_base);
+	if ((ctlr & CTLR_ENABLE_G0_BIT) == 0U) {
+		gicd_write_ctlr(driver_data->gicd_base,
+				ctlr | CTLR_ENABLE_G0_BIT);
+	}
+}
+
+/*******************************************************************************
+ * Global gic distributor init which will be done by the primary cpu after a
+ * cold boot. It marks out the secure SPIs, PPIs & SGIs and enables them. It
+ * then enables the secure GIC distributor interface.
+ ******************************************************************************/
+void gicv2_distif_init(void)
+{
+	unsigned int ctlr;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+
+	/* Disable the distributor before going further */
+	ctlr = gicd_read_ctlr(driver_data->gicd_base);
+	gicd_write_ctlr(driver_data->gicd_base,
+			ctlr & ~(CTLR_ENABLE_G0_BIT | CTLR_ENABLE_G1_BIT));
+
+	/* Set the default attribute of all SPIs */
+	gicv2_spis_configure_defaults(driver_data->gicd_base);
+
+	gicv2_secure_spis_configure_props(driver_data->gicd_base,
+			driver_data->interrupt_props,
+			driver_data->interrupt_props_num);
+
+
+	/* Re-enable the secure SPIs now that they have been configured */
+	gicd_write_ctlr(driver_data->gicd_base, ctlr | CTLR_ENABLE_G0_BIT);
+}
+
+/*******************************************************************************
+ * Initialize the ARM GICv2 driver with the provided platform inputs
+ ******************************************************************************/
+void gicv2_driver_init(const gicv2_driver_data_t *plat_driver_data)
+{
+	unsigned int gic_version;
+
+	assert(plat_driver_data != NULL);
+	assert(plat_driver_data->gicd_base != 0U);
+	assert(plat_driver_data->gicc_base != 0U);
+
+	assert(plat_driver_data->interrupt_props_num > 0 ?
+			plat_driver_data->interrupt_props != NULL : 1);
+
+	/* Ensure that this is a GICv2 system */
+	gic_version = gicd_read_pidr2(plat_driver_data->gicd_base);
+	gic_version = (gic_version >> PIDR2_ARCH_REV_SHIFT)
+					& PIDR2_ARCH_REV_MASK;
+
+	/*
+	 * GICv1 with security extension complies with trusted firmware
+	 * GICv2 driver as far as virtualization and few tricky power
+	 * features are not used. GICv2 features that are not supported
+	 * by GICv1 with Security Extensions are:
+	 * - virtual interrupt support.
+	 * - wake up events.
+	 * - writeable GIC state register (for power sequences)
+	 * - interrupt priority drop.
+	 * - interrupt signal bypass.
+	 */
+	assert((gic_version == ARCH_REV_GICV2) ||
+	       (gic_version == ARCH_REV_GICV1));
+
+	driver_data = plat_driver_data;
+
+	/*
+	 * The GIC driver data is initialized by the primary CPU with caches
+	 * enabled. When the secondary CPU boots up, it initializes the
+	 * GICC/GICR interface with the caches disabled. Hence flush the
+	 * driver_data to ensure coherency. This is not required if the
+	 * platform has HW_ASSISTED_COHERENCY or WARMBOOT_ENABLE_DCACHE_EARLY
+	 * enabled.
+	 */
+#if !(HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY)
+	flush_dcache_range((uintptr_t) &driver_data, sizeof(driver_data));
+	flush_dcache_range((uintptr_t) driver_data, sizeof(*driver_data));
+#endif
+	INFO("ARM GICv2 driver initialized\n");
+}
+
+/******************************************************************************
+ * This function returns whether FIQ is enabled in the GIC CPU interface.
+ *****************************************************************************/
+unsigned int gicv2_is_fiq_enabled(void)
+{
+	unsigned int gicc_ctlr;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	gicc_ctlr = gicc_read_ctlr(driver_data->gicc_base);
+	return (gicc_ctlr >> FIQ_EN_SHIFT) & 0x1U;
+}
+
+/*******************************************************************************
+ * This function returns the type of the highest priority pending interrupt at
+ * the GIC cpu interface. The return values can be one of the following :
+ *   PENDING_G1_INTID   : The interrupt type is non secure Group 1.
+ *   0 - 1019           : The interrupt type is secure Group 0.
+ *   GIC_SPURIOUS_INTERRUPT : there is no pending interrupt with
+ *                            sufficient priority to be signaled
+ ******************************************************************************/
+unsigned int gicv2_get_pending_interrupt_type(void)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	return gicc_read_hppir(driver_data->gicc_base) & INT_ID_MASK;
+}
+
+/*******************************************************************************
+ * This function returns the id of the highest priority pending interrupt at
+ * the GIC cpu interface. GIC_SPURIOUS_INTERRUPT is returned when there is no
+ * interrupt pending.
+ ******************************************************************************/
+unsigned int gicv2_get_pending_interrupt_id(void)
+{
+	unsigned int id;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	id = gicc_read_hppir(driver_data->gicc_base) & INT_ID_MASK;
+
+	/*
+	 * Find out which non-secure interrupt it is under the assumption that
+	 * the GICC_CTLR.AckCtl bit is 0.
+	 */
+	if (id == PENDING_G1_INTID)
+		id = gicc_read_ahppir(driver_data->gicc_base) & INT_ID_MASK;
+
+	return id;
+}
+
+/*******************************************************************************
+ * This functions reads the GIC cpu interface Interrupt Acknowledge register
+ * to start handling the pending secure 0 interrupt. It returns the
+ * contents of the IAR.
+ ******************************************************************************/
+unsigned int gicv2_acknowledge_interrupt(void)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	return gicc_read_IAR(driver_data->gicc_base);
+}
+
+/*******************************************************************************
+ * This functions writes the GIC cpu interface End Of Interrupt register with
+ * the passed value to finish handling the active secure group 0 interrupt.
+ ******************************************************************************/
+void gicv2_end_of_interrupt(unsigned int id)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	/*
+	 * Ensure the write to peripheral registers are *complete* before the write
+	 * to GIC_EOIR.
+	 *
+	 * Note: The completion gurantee depends on various factors of system design
+	 * and the barrier is the best core can do by which execution of further
+	 * instructions waits till the barrier is alive.
+	 */
+	dsbishst();
+	gicc_write_EOIR(driver_data->gicc_base, id);
+}
+
+/*******************************************************************************
+ * This function returns the type of the interrupt id depending upon the group
+ * this interrupt has been configured under by the interrupt controller i.e.
+ * group0 secure or group1 non secure. It returns zero for Group 0 secure and
+ * one for Group 1 non secure interrupt.
+ ******************************************************************************/
+unsigned int gicv2_get_interrupt_group(unsigned int id)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+
+	return gicd_get_igroupr(driver_data->gicd_base, id);
+}
+
+/*******************************************************************************
+ * This function returns the priority of the interrupt the processor is
+ * currently servicing.
+ ******************************************************************************/
+unsigned int gicv2_get_running_priority(void)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	return gicc_read_rpr(driver_data->gicc_base);
+}
+
+/*******************************************************************************
+ * This function sets the GICv2 target mask pattern for the current PE. The PE
+ * target mask is used to translate linear PE index (returned by platform core
+ * position) to a bit mask used when targeting interrupts to a PE (for example
+ * when raising SGIs and routing SPIs).
+ ******************************************************************************/
+void gicv2_set_pe_target_mask(unsigned int proc_num)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+	assert(driver_data->target_masks != NULL);
+	assert(proc_num < GICV2_MAX_TARGET_PE);
+	assert(proc_num < driver_data->target_masks_num);
+
+	/* Return if the target mask is already populated */
+	if (driver_data->target_masks[proc_num] != 0U)
+		return;
+
+	/*
+	 * Update target register corresponding to this CPU and flush for it to
+	 * be visible to other CPUs.
+	 */
+	if (driver_data->target_masks[proc_num] == 0U) {
+		driver_data->target_masks[proc_num] =
+			gicv2_get_cpuif_id(driver_data->gicd_base);
+#if !(HW_ASSISTED_COHERENCY || WARMBOOT_ENABLE_DCACHE_EARLY)
+		/*
+		 * PEs only update their own masks. Primary updates it with
+		 * caches on. But because secondaries does it with caches off,
+		 * all updates go to memory directly, and there's no danger of
+		 * secondaries overwriting each others' mask, despite
+		 * target_masks[] not being cache line aligned.
+		 */
+		flush_dcache_range((uintptr_t)
+				&driver_data->target_masks[proc_num],
+				sizeof(driver_data->target_masks[proc_num]));
+#endif
+	}
+}
+
+/*******************************************************************************
+ * This function returns the active status of the interrupt (either because the
+ * state is active, or active and pending).
+ ******************************************************************************/
+unsigned int gicv2_get_interrupt_active(unsigned int id)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+	assert(id <= MAX_SPI_ID);
+
+	return gicd_get_isactiver(driver_data->gicd_base, id);
+}
+
+/*******************************************************************************
+ * This function enables the interrupt identified by id.
+ ******************************************************************************/
+void gicv2_enable_interrupt(unsigned int id)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+	assert(id <= MAX_SPI_ID);
+
+	/*
+	 * Ensure that any shared variable updates depending on out of band
+	 * interrupt trigger are observed before enabling interrupt.
+	 */
+	dsbishst();
+	gicd_set_isenabler(driver_data->gicd_base, id);
+}
+
+/*******************************************************************************
+ * This function disables the interrupt identified by id.
+ ******************************************************************************/
+void gicv2_disable_interrupt(unsigned int id)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+	assert(id <= MAX_SPI_ID);
+
+	/*
+	 * Disable interrupt, and ensure that any shared variable updates
+	 * depending on out of band interrupt trigger are observed afterwards.
+	 */
+	gicd_set_icenabler(driver_data->gicd_base, id);
+	dsbishst();
+}
+
+/*******************************************************************************
+ * This function sets the interrupt priority as supplied for the given interrupt
+ * id.
+ ******************************************************************************/
+void gicv2_set_interrupt_priority(unsigned int id, unsigned int priority)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+	assert(id <= MAX_SPI_ID);
+
+	gicd_set_ipriorityr(driver_data->gicd_base, id, priority);
+}
+
+/*******************************************************************************
+ * This function assigns group for the interrupt identified by id. The group can
+ * be any of GICV2_INTR_GROUP*
+ ******************************************************************************/
+void gicv2_set_interrupt_type(unsigned int id, unsigned int type)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+	assert(id <= MAX_SPI_ID);
+
+	/* Serialize read-modify-write to Distributor registers */
+	spin_lock(&gic_lock);
+	switch (type) {
+	case GICV2_INTR_GROUP1:
+		gicd_set_igroupr(driver_data->gicd_base, id);
+		break;
+	case GICV2_INTR_GROUP0:
+		gicd_clr_igroupr(driver_data->gicd_base, id);
+		break;
+	default:
+		assert(false);
+		break;
+	}
+	spin_unlock(&gic_lock);
+}
+
+/*******************************************************************************
+ * This function raises the specified SGI to requested targets.
+ *
+ * The proc_num parameter must be the linear index of the target PE in the
+ * system.
+ ******************************************************************************/
+void gicv2_raise_sgi(int sgi_num, bool ns, int proc_num)
+{
+	unsigned int sgir_val, target;
+
+	assert(driver_data != NULL);
+	assert(proc_num >= 0);
+	assert(proc_num < (int)GICV2_MAX_TARGET_PE);
+	assert(driver_data->gicd_base != 0U);
+
+	/*
+	 * Target masks array must have been supplied, and the core position
+	 * should be valid.
+	 */
+	assert(driver_data->target_masks != NULL);
+	assert(proc_num < (int)driver_data->target_masks_num);
+
+	/* Don't raise SGI if the mask hasn't been populated */
+	target = driver_data->target_masks[proc_num];
+	assert(target != 0U);
+
+	sgir_val = GICV2_SGIR_VALUE(SGIR_TGT_SPECIFIC, target, ns, sgi_num);
+
+	/*
+	 * Ensure that any shared variable updates depending on out of band
+	 * interrupt trigger are observed before raising SGI.
+	 */
+	dsbishst();
+	gicd_write_sgir(driver_data->gicd_base, sgir_val);
+}
+
+/*******************************************************************************
+ * This function sets the interrupt routing for the given SPI interrupt id.
+ * The interrupt routing is specified in routing mode. The proc_num parameter is
+ * linear index of the PE to target SPI. When proc_num < 0, the SPI may target
+ * all PEs.
+ ******************************************************************************/
+void gicv2_set_spi_routing(unsigned int id, int proc_num)
+{
+	unsigned int target;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+
+	assert((id >= MIN_SPI_ID) && (id <= MAX_SPI_ID));
+
+	/*
+	 * Target masks array must have been supplied, and the core position
+	 * should be valid.
+	 */
+	assert(driver_data->target_masks != NULL);
+	assert(proc_num < (int)GICV2_MAX_TARGET_PE);
+	assert(driver_data->target_masks_num < INT_MAX);
+	assert(proc_num < (int)driver_data->target_masks_num);
+
+	if (proc_num < 0) {
+		/* Target all PEs */
+		target = GIC_TARGET_CPU_MASK;
+	} else {
+		/* Don't route interrupt if the mask hasn't been populated */
+		target = driver_data->target_masks[proc_num];
+		assert(target != 0U);
+	}
+
+	gicd_set_itargetsr(driver_data->gicd_base, id, target);
+}
+
+/*******************************************************************************
+ * This function clears the pending status of an interrupt identified by id.
+ ******************************************************************************/
+void gicv2_clear_interrupt_pending(unsigned int id)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+
+	/* SGIs can't be cleared pending */
+	assert(id >= MIN_PPI_ID);
+
+	/*
+	 * Clear pending interrupt, and ensure that any shared variable updates
+	 * depending on out of band interrupt trigger are observed afterwards.
+	 */
+	gicd_set_icpendr(driver_data->gicd_base, id);
+	dsbishst();
+}
+
+/*******************************************************************************
+ * This function sets the pending status of an interrupt identified by id.
+ ******************************************************************************/
+void gicv2_set_interrupt_pending(unsigned int id)
+{
+	assert(driver_data != NULL);
+	assert(driver_data->gicd_base != 0U);
+
+	/* SGIs can't be cleared pending */
+	assert(id >= MIN_PPI_ID);
+
+	/*
+	 * Ensure that any shared variable updates depending on out of band
+	 * interrupt trigger are observed before setting interrupt pending.
+	 */
+	dsbishst();
+	gicd_set_ispendr(driver_data->gicd_base, id);
+}
+
+/*******************************************************************************
+ * This function sets the PMR register with the supplied value. Returns the
+ * original PMR.
+ ******************************************************************************/
+unsigned int gicv2_set_pmr(unsigned int mask)
+{
+	unsigned int old_mask;
+
+	assert(driver_data != NULL);
+	assert(driver_data->gicc_base != 0U);
+
+	old_mask = gicc_read_pmr(driver_data->gicc_base);
+
+	/*
+	 * Order memory updates w.r.t. PMR write, and ensure they're visible
+	 * before potential out of band interrupt trigger because of PMR update.
+	 */
+	dmbishst();
+	gicc_write_pmr(driver_data->gicc_base, mask);
+	dsbishst();
+
+	return old_mask;
+}
+
+/*******************************************************************************
+ * This function updates single interrupt configuration to be level/edge
+ * triggered
+ ******************************************************************************/
+void gicv2_interrupt_set_cfg(unsigned int id, unsigned int cfg)
+{
+	gicd_set_icfgr(driver_data->gicd_base, id, cfg);
+}
diff --git a/drivers/arm/gic/v2/gicv2_private.h b/drivers/arm/gic/v2/gicv2_private.h
new file mode 100644
index 0000000..0fbdab0
--- /dev/null
+++ b/drivers/arm/gic/v2/gicv2_private.h
@@ -0,0 +1,150 @@
+/*
+ * Copyright (c) 2015-2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef GICV2_PRIVATE_H
+#define GICV2_PRIVATE_H
+
+#include <stdint.h>
+
+#include <drivers/arm/gicv2.h>
+#include <lib/mmio.h>
+
+/*******************************************************************************
+ * Private function prototypes
+ ******************************************************************************/
+void gicv2_spis_configure_defaults(uintptr_t gicd_base);
+void gicv2_secure_spis_configure_props(uintptr_t gicd_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num);
+void gicv2_secure_ppi_sgi_setup_props(uintptr_t gicd_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num);
+unsigned int gicv2_get_cpuif_id(uintptr_t base);
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading entire registers
+ ******************************************************************************/
+static inline unsigned int gicd_read_pidr2(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_PIDR2_GICV2);
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for writing entire registers
+ ******************************************************************************/
+static inline unsigned int gicd_get_itargetsr(uintptr_t base, unsigned int id)
+{
+	return mmio_read_8(base + GICD_ITARGETSR + id);
+}
+
+static inline void gicd_set_itargetsr(uintptr_t base, unsigned int id,
+		unsigned int target)
+{
+	uint8_t val = target & GIC_TARGET_CPU_MASK;
+
+	mmio_write_8(base + GICD_ITARGETSR + id, val);
+}
+
+static inline void gicd_write_sgir(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICD_SGIR, val);
+}
+
+/*******************************************************************************
+ * GIC CPU interface accessors for reading entire registers
+ ******************************************************************************/
+
+static inline unsigned int gicc_read_ctlr(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_CTLR);
+}
+
+static inline unsigned int gicc_read_pmr(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_PMR);
+}
+
+static inline unsigned int gicc_read_BPR(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_BPR);
+}
+
+static inline unsigned int gicc_read_IAR(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_IAR);
+}
+
+static inline unsigned int gicc_read_EOIR(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_EOIR);
+}
+
+static inline unsigned int gicc_read_hppir(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_HPPIR);
+}
+
+static inline unsigned int gicc_read_ahppir(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_AHPPIR);
+}
+
+static inline unsigned int gicc_read_dir(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_DIR);
+}
+
+static inline unsigned int gicc_read_iidr(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_IIDR);
+}
+
+static inline unsigned int gicc_read_rpr(uintptr_t base)
+{
+	return mmio_read_32(base + GICC_RPR);
+}
+
+/*******************************************************************************
+ * GIC CPU interface accessors for writing entire registers
+ ******************************************************************************/
+
+static inline void gicc_write_ctlr(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICC_CTLR, val);
+}
+
+static inline void gicc_write_pmr(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICC_PMR, val);
+}
+
+static inline void gicc_write_BPR(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICC_BPR, val);
+}
+
+
+static inline void gicc_write_IAR(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICC_IAR, val);
+}
+
+static inline void gicc_write_EOIR(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICC_EOIR, val);
+}
+
+static inline void gicc_write_hppir(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICC_HPPIR, val);
+}
+
+static inline void gicc_write_dir(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICC_DIR, val);
+}
+
+#endif /* GICV2_PRIVATE_H */
diff --git a/drivers/arm/gic/v3/arm_gicv3_common.c b/drivers/arm/gic/v3/arm_gicv3_common.c
new file mode 100644
index 0000000..4489892
--- /dev/null
+++ b/drivers/arm/gic/v3/arm_gicv3_common.c
@@ -0,0 +1,115 @@
+/*
+ * Copyright (c) 2017, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Driver for implementation defined features that are identical in ARM GICv3
+* implementations (GIC-500 and GIC-600 for now). This driver only overrides
+* APIs that are different to those generic ones in GICv3 driver.
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <drivers/arm/arm_gicv3_common.h>
+#include <drivers/arm/gicv3.h>
+
+#include "gicv3_private.h"
+
+/*
+ * Flush the internal GIC cache of the LPIs pending tables to memory before
+ * saving the state of the Redistributor. This is required before powering off
+ * the GIC when the pending status must be preserved.
+ * `rdist_proc_num` is the processor number corresponding to the Redistributor of the
+ * current CPU.
+ */
+void arm_gicv3_distif_pre_save(unsigned int rdist_proc_num)
+{
+	uintptr_t gicr_base = 0;
+
+	assert(gicv3_driver_data);
+	assert(gicv3_driver_data->rdistif_base_addrs);
+
+	/*
+	 * The GICR_WAKER.Sleep bit should be set only when both
+	 * GICR_WAKER.ChildrenAsleep and GICR_WAKER.ProcessorSleep are set on
+	 * all the Redistributors.
+	 */
+	for (unsigned int i = 0; i < gicv3_driver_data->rdistif_num; i++) {
+		gicr_base = gicv3_driver_data->rdistif_base_addrs[i];
+		assert(gicr_base);
+		assert(gicr_read_waker(gicr_base) & WAKER_CA_BIT);
+		assert(gicr_read_waker(gicr_base) & WAKER_PS_BIT);
+	}
+
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[rdist_proc_num];
+	/*
+	 * According to the TRM, there is only one instance of the
+	 * GICR_WAKER.Sleep and GICR_WAKER.Quiescent bits that can be accessed
+	 * through any of the Redistributor.
+	 */
+
+	/*
+	 * Set GICR_WAKER.Sleep
+	 * After this point, the system must be configured so that the
+	 * wake_request signals for the right cores are asserted when a wakeup
+	 * interrupt is detected. The GIC will not be able to do that anymore
+	 * when the GICR_WAKER.Sleep bit is set to 1.
+	 */
+	gicr_write_waker(gicr_base, gicr_read_waker(gicr_base) | WAKER_SL_BIT);
+
+	/* Wait until the GICR_WAKER.Quiescent bit is set */
+	while (!(gicr_read_waker(gicr_base) & WAKER_QSC_BIT))
+		;
+}
+
+/*
+ * Allow the LPIs pending state to be read back from the tables in memory after
+ * having restored the state of the GIC Redistributor.
+ */
+void arm_gicv3_distif_post_restore(unsigned int rdist_proc_num)
+{
+	uintptr_t gicr_base;
+
+	assert(gicv3_driver_data);
+	assert(gicv3_driver_data->rdistif_base_addrs);
+
+	/*
+	 * According to the TRM, there is only one instance of the
+	 * GICR_WAKER.Sleep and GICR_WAKER.Quiescent bits that can be accessed
+	 * through any of the Redistributor.
+	 */
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[rdist_proc_num];
+	assert(gicr_base);
+
+	/*
+	 * If the GIC had power removed, the GICR_WAKER state will be reset.
+	 * Since the GICR_WAKER.Sleep and GICR_WAKER.Quiescent bits are cleared,
+	 * we can exit early. This also prevents the following assert from
+	 * erroneously triggering.
+	 */
+	if (!(gicr_read_waker(gicr_base) & WAKER_SL_BIT))
+		return;
+
+	/*
+	 * Writes to GICR_WAKER.Sleep bit are ignored if GICR_WAKER.Quiescent
+	 * bit is not set. We should be alright on power on path, therefore
+	 * coming out of sleep and Quiescent should be set, but we assert in
+	 * case.
+	 */
+	assert(gicr_read_waker(gicr_base) & WAKER_QSC_BIT);
+
+	/* Clear GICR_WAKER.Sleep */
+	gicr_write_waker(gicr_base, gicr_read_waker(gicr_base) & ~WAKER_SL_BIT);
+
+	/*
+	 * We don't know if the effects of setting GICR_WAKER.Sleep bit is
+	 * instantaneous, so we wait until the interface is not Quiescent
+	 * anymore.
+	 */
+	while (gicr_read_waker(gicr_base) & WAKER_QSC_BIT)
+		;
+}
+
diff --git a/drivers/arm/gic/v3/gic-x00.c b/drivers/arm/gic/v3/gic-x00.c
new file mode 100644
index 0000000..83ef32f
--- /dev/null
+++ b/drivers/arm/gic/v3/gic-x00.c
@@ -0,0 +1,232 @@
+/*
+ * Copyright (c) 2017-2022, Arm Limited and Contributors. All rights reserved.
+ * Copyright (c) 2020, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Driver for GIC-500 and GIC-600 specific features. This driver only
+ * overrides APIs that are different to those generic ones in GICv3
+ * driver.
+ *
+ * GIC-600 supports independently power-gating redistributor interface.
+ */
+
+#include <assert.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/arm_gicv3_common.h>
+#include <drivers/arm/gicv3.h>
+
+#include "gicv3_private.h"
+
+/* GIC-600 specific register offsets */
+#define GICR_PWRR			0x24U
+
+/* GICR_PWRR fields */
+#define PWRR_RDPD_SHIFT			0
+#define PWRR_RDAG_SHIFT			1
+#define PWRR_RDGPD_SHIFT		2
+#define PWRR_RDGPO_SHIFT		3
+
+#define PWRR_RDPD			(1U << PWRR_RDPD_SHIFT)
+#define PWRR_RDAG			(1U << PWRR_RDAG_SHIFT)
+#define PWRR_RDGPD			(1U << PWRR_RDGPD_SHIFT)
+#define PWRR_RDGPO			(1U << PWRR_RDGPO_SHIFT)
+
+/*
+ * Values to write to GICR_PWRR register to power redistributor
+ * for operating through the core (GICR_PWRR.RDAG = 0)
+ */
+#define PWRR_ON				(0U << PWRR_RDPD_SHIFT)
+#define PWRR_OFF			(1U << PWRR_RDPD_SHIFT)
+
+static bool gic600_errata_wa_2384374 __unused;
+
+#if GICV3_SUPPORT_GIC600
+
+/* GIC-600/700 specific accessor functions */
+static void gicr_write_pwrr(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_PWRR, val);
+}
+
+static uint32_t gicr_read_pwrr(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_PWRR);
+}
+
+static void gicr_wait_group_not_in_transit(uintptr_t base)
+{
+	uint32_t pwrr;
+
+	do {
+		pwrr = gicr_read_pwrr(base);
+
+	/* Check group not transitioning: RDGPD == RDGPO */
+	} while (((pwrr & PWRR_RDGPD) >> PWRR_RDGPD_SHIFT) !=
+		 ((pwrr & PWRR_RDGPO) >> PWRR_RDGPO_SHIFT));
+}
+
+static void gic600_pwr_on(uintptr_t base)
+{
+	do {	/* Wait until group not transitioning */
+		gicr_wait_group_not_in_transit(base);
+
+		/* Power on redistributor */
+		gicr_write_pwrr(base, PWRR_ON);
+
+		/*
+		 * Wait until the power on state is reflected.
+		 * If RDPD == 0 then powered on.
+		 */
+	} while ((gicr_read_pwrr(base) & PWRR_RDPD) != PWRR_ON);
+}
+
+static void gic600_pwr_off(uintptr_t base)
+{
+	/* Wait until group not transitioning */
+	gicr_wait_group_not_in_transit(base);
+
+	/* Power off redistributor */
+	gicr_write_pwrr(base, PWRR_OFF);
+
+	/*
+	 * If this is the last man, turning this redistributor frame off will
+	 * result in the group itself being powered off and RDGPD = 1.
+	 * In that case, wait as long as it's in transition, or has aborted
+	 * the transition altogether for any reason.
+	 */
+	if ((gicr_read_pwrr(base) & PWRR_RDGPD) != 0U) {
+		/* Wait until group not transitioning */
+		gicr_wait_group_not_in_transit(base);
+	}
+}
+
+static uintptr_t get_gicr_base(unsigned int proc_num)
+{
+	uintptr_t gicr_base;
+
+	assert(gicv3_driver_data != NULL);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+	assert(gicr_base != 0UL);
+
+	return gicr_base;
+}
+
+static bool gicv3_redists_need_power_mgmt(uintptr_t gicr_base)
+{
+	uint32_t reg = mmio_read_32(gicr_base + GICR_IIDR);
+
+	/*
+	 * The Arm GIC-600 and GIC-700 models have their redistributors
+	 * powered down at reset.
+	 */
+	return (((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600) ||
+		((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600AE) ||
+		((reg & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_700));
+}
+
+#endif	/* GICV3_SUPPORT_GIC600 */
+
+void gicv3_distif_pre_save(unsigned int proc_num)
+{
+	arm_gicv3_distif_pre_save(proc_num);
+}
+
+void gicv3_distif_post_restore(unsigned int proc_num)
+{
+	arm_gicv3_distif_post_restore(proc_num);
+}
+
+/*
+ * Power off GIC-600 redistributor (if configured and detected)
+ */
+void gicv3_rdistif_off(unsigned int proc_num)
+{
+#if GICV3_SUPPORT_GIC600
+	uintptr_t gicr_base = get_gicr_base(proc_num);
+
+	/* Attempt to power redistributor off */
+	if (gicv3_redists_need_power_mgmt(gicr_base)) {
+		gic600_pwr_off(gicr_base);
+	}
+#endif
+}
+
+/*
+ * Power on GIC-600 redistributor (if configured and detected)
+ */
+void gicv3_rdistif_on(unsigned int proc_num)
+{
+#if GICV3_SUPPORT_GIC600
+	uintptr_t gicr_base = get_gicr_base(proc_num);
+
+	/* Power redistributor on */
+	if (gicv3_redists_need_power_mgmt(gicr_base)) {
+		gic600_pwr_on(gicr_base);
+	}
+#endif
+}
+
+#if GIC600_ERRATA_WA_2384374
+/*******************************************************************************
+ * Apply part 2 of workaround for errata-2384374 as per SDEN:
+ * https://developer.arm.com/documentation/sden892601/latest/
+ ******************************************************************************/
+void gicv3_apply_errata_wa_2384374(uintptr_t gicr_base)
+{
+	if (gic600_errata_wa_2384374) {
+		uint32_t gicr_ctlr_val = gicr_read_ctlr(gicr_base);
+
+		gicr_write_ctlr(gicr_base, gicr_ctlr_val |
+				(GICR_CTLR_DPG0_BIT | GICR_CTLR_DPG1NS_BIT |
+				GICR_CTLR_DPG1S_BIT));
+		gicr_write_ctlr(gicr_base, gicr_ctlr_val &
+				~(GICR_CTLR_DPG0_BIT | GICR_CTLR_DPG1NS_BIT |
+				  GICR_CTLR_DPG1S_BIT));
+	}
+}
+#endif /* GIC600_ERRATA_WA_2384374 */
+
+void gicv3_check_erratas_applies(uintptr_t gicd_base)
+{
+	unsigned int gic_prod_id;
+	uint8_t gic_rev;
+
+	assert(gicd_base != 0UL);
+
+	gicv3_get_component_prodid_rev(gicd_base, &gic_prod_id, &gic_rev);
+
+	/*
+	 * This workaround applicable only to GIC600 and GIC600AE products with
+	 * revision less than r1p6 and r0p2 respectively.
+	 * As per GIC600/GIC600AE specification -
+	 * r1p6 = 0x17 => GICD_IIDR[19:12]
+	 * r0p2 = 0x04 => GICD_IIDR[19:12]
+	 */
+	if ((gic_prod_id == GIC_PRODUCT_ID_GIC600) ||
+		    (gic_prod_id == GIC_PRODUCT_ID_GIC600AE)) {
+		if (((gic_prod_id == GIC_PRODUCT_ID_GIC600) &&
+		     (gic_rev <= GIC_REV(GIC_VARIANT_R1, GIC_REV_P6))) ||
+		     ((gic_prod_id == GIC_PRODUCT_ID_GIC600AE) &&
+		     (gic_rev <= GIC_REV(GIC_VARIANT_R0, GIC_REV_P2)))) {
+#if GIC600_ERRATA_WA_2384374
+			gic600_errata_wa_2384374 = true;
+			VERBOSE("%s applies\n",
+				"GIC600/GIC600AE errata workaround 2384374");
+#else
+			WARN("%s missing\n",
+			     "GIC600/GIC600AE errata workaround 2384374");
+#endif /* GIC600_ERRATA_WA_2384374 */
+		} else {
+			VERBOSE("%s not applies\n",
+				"GIC600/GIC600AE errata workaround 2384374");
+		}
+	}
+}
diff --git a/drivers/arm/gic/v3/gic600_multichip.c b/drivers/arm/gic/v3/gic600_multichip.c
new file mode 100644
index 0000000..e85dbc1
--- /dev/null
+++ b/drivers/arm/gic/v3/gic600_multichip.c
@@ -0,0 +1,358 @@
+/*
+ * Copyright (c) 2019, Arm Limited. All rights reserved.
+ * Copyright (c) 2022, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * GIC-600 driver extension for multichip setup
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <drivers/arm/arm_gicv3_common.h>
+#include <drivers/arm/gic600_multichip.h>
+#include <drivers/arm/gicv3.h>
+
+#include "../common/gic_common_private.h"
+#include "gic600_multichip_private.h"
+
+/*******************************************************************************
+ * GIC-600 multichip operation related helper functions
+ ******************************************************************************/
+static void gicd_dchipr_wait_for_power_update_progress(uintptr_t base)
+{
+	unsigned int retry = GICD_PUP_UPDATE_RETRIES;
+
+	while ((read_gicd_dchipr(base) & GICD_DCHIPR_PUP_BIT) != 0U) {
+		if (retry-- == 0) {
+			ERROR("GIC-600 connection to Routing Table Owner timed "
+					 "out\n");
+			panic();
+		}
+	}
+}
+
+/*******************************************************************************
+ * Sets up the routing table owner.
+ ******************************************************************************/
+static void set_gicd_dchipr_rt_owner(uintptr_t base, unsigned int rt_owner)
+{
+	/*
+	 * Ensure that Group enables in GICD_CTLR are disabled and no pending
+	 * register writes to GICD_CTLR.
+	 */
+	if ((gicd_read_ctlr(base) &
+			(CTLR_ENABLE_G0_BIT | CTLR_ENABLE_G1S_BIT |
+			 CTLR_ENABLE_G1NS_BIT | GICD_CTLR_RWP_BIT)) != 0) {
+		ERROR("GICD_CTLR group interrupts are either enabled or have "
+				"pending writes. Cannot set RT owner.\n");
+		panic();
+	}
+
+	/* Poll till PUP is zero before intiating write */
+	gicd_dchipr_wait_for_power_update_progress(base);
+
+	write_gicd_dchipr(base, read_gicd_dchipr(base) |
+			(rt_owner << GICD_DCHIPR_RT_OWNER_SHIFT));
+
+	/* Poll till PUP is zero to ensure write is complete */
+	gicd_dchipr_wait_for_power_update_progress(base);
+}
+
+/*******************************************************************************
+ * Configures the Chip Register to make connections to GICDs on
+ * a multichip platform.
+ ******************************************************************************/
+static void set_gicd_chipr_n(uintptr_t base,
+				unsigned int chip_id,
+				uint64_t chip_addr,
+				unsigned int spi_id_min,
+				unsigned int spi_id_max)
+{
+	unsigned int spi_block_min, spi_blocks;
+	unsigned int gicd_iidr_val = gicd_read_iidr(base);
+	uint64_t chipr_n_val;
+
+	/*
+	 * Ensure that group enables in GICD_CTLR are disabled and no pending
+	 * register writes to GICD_CTLR.
+	 */
+	if ((gicd_read_ctlr(base) &
+			(CTLR_ENABLE_G0_BIT | CTLR_ENABLE_G1S_BIT |
+			 CTLR_ENABLE_G1NS_BIT | GICD_CTLR_RWP_BIT)) != 0) {
+		ERROR("GICD_CTLR group interrupts are either enabled or have "
+				"pending writes. Cannot set CHIPR register.\n");
+		panic();
+	}
+
+	/*
+	 * spi_id_min and spi_id_max of value 0 is used to intidicate that the
+	 * chip doesn't own any SPI block. Re-assign min and max values as SPI
+	 * id starts from 32.
+	 */
+	if (spi_id_min == 0 && spi_id_max == 0) {
+		spi_id_min = GIC600_SPI_ID_MIN;
+		spi_id_max = GIC600_SPI_ID_MIN;
+	}
+
+	switch ((gicd_iidr_val & IIDR_MODEL_MASK)) {
+	case IIDR_MODEL_ARM_GIC_600:
+		spi_block_min = SPI_BLOCK_MIN_VALUE(spi_id_min);
+		spi_blocks    = SPI_BLOCKS_VALUE(spi_id_min, spi_id_max);
+
+		chipr_n_val = GICD_CHIPR_VALUE_GIC_600(chip_addr,
+						       spi_block_min,
+						       spi_blocks);
+		break;
+	case IIDR_MODEL_ARM_GIC_700:
+		/* Calculate the SPI_ID_MIN value for ESPI */
+		if (spi_id_min >= GIC700_ESPI_ID_MIN) {
+			spi_block_min = ESPI_BLOCK_MIN_VALUE(spi_id_min);
+			spi_block_min += SPI_BLOCKS_VALUE(GIC700_SPI_ID_MIN,
+				GIC700_SPI_ID_MAX);
+		} else {
+			spi_block_min = SPI_BLOCK_MIN_VALUE(spi_id_min);
+		}
+
+		/* Calculate the total number of blocks */
+		spi_blocks = SPI_BLOCKS_VALUE(spi_id_min, spi_id_max);
+
+		chipr_n_val = GICD_CHIPR_VALUE_GIC_700(chip_addr,
+						       spi_block_min,
+						       spi_blocks);
+		break;
+	default:
+		ERROR("Unsupported GIC model 0x%x for multichip setup.\n",
+		      gicd_iidr_val);
+		panic();
+		break;
+	}
+	chipr_n_val |= GICD_CHIPRx_SOCKET_STATE;
+
+	/*
+	 * Wait for DCHIPR.PUP to be zero before commencing writes to
+	 * GICD_CHIPRx.
+	 */
+	gicd_dchipr_wait_for_power_update_progress(base);
+
+	/*
+	 * Assign chip addr, spi min block, number of spi blocks and bring chip
+	 * online by setting SocketState.
+	 */
+	write_gicd_chipr_n(base, chip_id, chipr_n_val);
+
+	/*
+	 * Poll until DCHIP.PUP is zero to verify connection to rt_owner chip
+	 * is complete.
+	 */
+	gicd_dchipr_wait_for_power_update_progress(base);
+
+	/*
+	 * Ensure that write to GICD_CHIPRx is successful and the chip_n came
+	 * online.
+	 */
+	if (read_gicd_chipr_n(base, chip_id) != chipr_n_val) {
+		ERROR("GICD_CHIPR%u write failed\n", chip_id);
+		panic();
+	}
+
+	/* Ensure that chip is in consistent state */
+	if (((read_gicd_chipsr(base) & GICD_CHIPSR_RTS_MASK) >>
+				GICD_CHIPSR_RTS_SHIFT) !=
+			GICD_CHIPSR_RTS_STATE_CONSISTENT) {
+		ERROR("Chip %u routing table is not in consistent state\n",
+				chip_id);
+		panic();
+	}
+}
+
+/*******************************************************************************
+ * Validates the GIC-600 Multichip data structure passed by the platform.
+ ******************************************************************************/
+static void gic600_multichip_validate_data(
+		struct gic600_multichip_data *multichip_data)
+{
+	unsigned int i, spi_id_min, spi_id_max, blocks_of_32;
+	unsigned int multichip_spi_blocks = 0;
+
+	assert(multichip_data != NULL);
+
+	if (multichip_data->chip_count > GIC600_MAX_MULTICHIP) {
+		ERROR("GIC-600 Multichip count should not exceed %d\n",
+				GIC600_MAX_MULTICHIP);
+		panic();
+	}
+
+	for (i = 0; i < multichip_data->chip_count; i++) {
+		spi_id_min = multichip_data->spi_ids[i][SPI_MIN_INDEX];
+		spi_id_max = multichip_data->spi_ids[i][SPI_MAX_INDEX];
+
+		if ((spi_id_min != 0) || (spi_id_max != 0)) {
+
+			/* SPI IDs range check */
+			if (!(spi_id_min >= GIC600_SPI_ID_MIN) ||
+			    !(spi_id_max < GIC600_SPI_ID_MAX) ||
+			    !(spi_id_min <= spi_id_max) ||
+			    !((spi_id_max - spi_id_min + 1) % 32 == 0)) {
+				ERROR("Invalid SPI IDs {%u, %u} passed for "
+						"Chip %u\n", spi_id_min,
+						spi_id_max, i);
+				panic();
+			}
+
+			/* SPI IDs overlap check */
+			blocks_of_32 = BLOCKS_OF_32(spi_id_min, spi_id_max);
+			if ((multichip_spi_blocks & blocks_of_32) != 0) {
+				ERROR("SPI IDs of Chip %u overlapping\n", i);
+				panic();
+			}
+			multichip_spi_blocks |= blocks_of_32;
+		}
+	}
+}
+
+/*******************************************************************************
+ * Validates the GIC-700 Multichip data structure passed by the platform.
+ ******************************************************************************/
+static void gic700_multichip_validate_data(
+		struct gic600_multichip_data *multichip_data)
+{
+	unsigned int i, spi_id_min, spi_id_max, blocks_of_32;
+	unsigned int multichip_spi_blocks = 0U, multichip_espi_blocks = 0U;
+
+	assert(multichip_data != NULL);
+
+	if (multichip_data->chip_count > GIC600_MAX_MULTICHIP) {
+		ERROR("GIC-700 Multichip count (%u) should not exceed %u\n",
+				multichip_data->chip_count, GIC600_MAX_MULTICHIP);
+		panic();
+	}
+
+	for (i = 0U; i < multichip_data->chip_count; i++) {
+		spi_id_min = multichip_data->spi_ids[i][SPI_MIN_INDEX];
+		spi_id_max = multichip_data->spi_ids[i][SPI_MAX_INDEX];
+
+		if ((spi_id_min == 0U) || (spi_id_max == 0U)) {
+			continue;
+		}
+
+		/* MIN SPI ID check */
+		if ((spi_id_min < GIC700_SPI_ID_MIN) ||
+		    ((spi_id_min >= GIC700_SPI_ID_MAX) &&
+		     (spi_id_min < GIC700_ESPI_ID_MIN))) {
+			ERROR("Invalid MIN SPI ID {%u} passed for "
+					"Chip %u\n", spi_id_min, i);
+			panic();
+		}
+
+		if ((spi_id_min > spi_id_max) ||
+		    ((spi_id_max - spi_id_min + 1) % 32 != 0)) {
+			ERROR("Unaligned SPI IDs {%u, %u} passed for "
+					"Chip %u\n", spi_id_min,
+					spi_id_max, i);
+			panic();
+		}
+
+		/* ESPI IDs range check */
+		if ((spi_id_min >= GIC700_ESPI_ID_MIN) &&
+		    (spi_id_max > GIC700_ESPI_ID_MAX)) {
+			ERROR("Invalid ESPI IDs {%u, %u} passed for "
+					"Chip %u\n", spi_id_min,
+					spi_id_max, i);
+			panic();
+
+		}
+
+		/* SPI IDs range check */
+		if (((spi_id_min < GIC700_SPI_ID_MAX) &&
+		     (spi_id_max > GIC700_SPI_ID_MAX))) {
+			ERROR("Invalid SPI IDs {%u, %u} passed for "
+					"Chip %u\n", spi_id_min,
+					spi_id_max, i);
+			panic();
+		}
+
+		/* SPI IDs overlap check */
+		if (spi_id_max < GIC700_SPI_ID_MAX) {
+			blocks_of_32 = BLOCKS_OF_32(spi_id_min, spi_id_max);
+			if ((multichip_spi_blocks & blocks_of_32) != 0) {
+				ERROR("SPI IDs of Chip %u overlapping\n", i);
+				panic();
+			}
+			multichip_spi_blocks |= blocks_of_32;
+		}
+
+		/* ESPI IDs overlap check */
+		if (spi_id_max > GIC700_ESPI_ID_MIN) {
+			blocks_of_32 = BLOCKS_OF_32(spi_id_min - GIC700_ESPI_ID_MIN,
+					spi_id_max - GIC700_ESPI_ID_MIN);
+			if ((multichip_espi_blocks & blocks_of_32) != 0) {
+				ERROR("SPI IDs of Chip %u overlapping\n", i);
+				panic();
+			}
+			multichip_espi_blocks |= blocks_of_32;
+		}
+	}
+}
+
+/*******************************************************************************
+ * Intialize GIC-600 and GIC-700 Multichip operation.
+ ******************************************************************************/
+void gic600_multichip_init(struct gic600_multichip_data *multichip_data)
+{
+	unsigned int i;
+	uint32_t gicd_iidr_val = gicd_read_iidr(multichip_data->rt_owner_base);
+
+	if ((gicd_iidr_val & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_600) {
+		gic600_multichip_validate_data(multichip_data);
+	}
+
+	if ((gicd_iidr_val & IIDR_MODEL_MASK) == IIDR_MODEL_ARM_GIC_700) {
+		gic700_multichip_validate_data(multichip_data);
+	}
+
+	/*
+	 * Ensure that G0/G1S/G1NS interrupts are disabled. This also ensures
+	 * that GIC-600 Multichip configuration is done first.
+	 */
+	if ((gicd_read_ctlr(multichip_data->rt_owner_base) &
+			(CTLR_ENABLE_G0_BIT | CTLR_ENABLE_G1S_BIT |
+			 CTLR_ENABLE_G1NS_BIT | GICD_CTLR_RWP_BIT)) != 0) {
+		ERROR("GICD_CTLR group interrupts are either enabled or have "
+				"pending writes.\n");
+		panic();
+	}
+
+	/* Ensure that the routing table owner is in disconnected state */
+	if (((read_gicd_chipsr(multichip_data->rt_owner_base) &
+		GICD_CHIPSR_RTS_MASK) >> GICD_CHIPSR_RTS_SHIFT) !=
+			GICD_CHIPSR_RTS_STATE_DISCONNECTED) {
+		ERROR("GIC-600 routing table owner is not in disconnected "
+				"state to begin multichip configuration\n");
+		panic();
+	}
+
+	/* Initialize the GICD which is marked as routing table owner first */
+	set_gicd_dchipr_rt_owner(multichip_data->rt_owner_base,
+			multichip_data->rt_owner);
+
+	set_gicd_chipr_n(multichip_data->rt_owner_base, multichip_data->rt_owner,
+			multichip_data->chip_addrs[multichip_data->rt_owner],
+			multichip_data->
+			spi_ids[multichip_data->rt_owner][SPI_MIN_INDEX],
+			multichip_data->
+			spi_ids[multichip_data->rt_owner][SPI_MAX_INDEX]);
+
+	for (i = 0; i < multichip_data->chip_count; i++) {
+		if (i == multichip_data->rt_owner)
+			continue;
+
+		set_gicd_chipr_n(multichip_data->rt_owner_base, i,
+				multichip_data->chip_addrs[i],
+				multichip_data->spi_ids[i][SPI_MIN_INDEX],
+				multichip_data->spi_ids[i][SPI_MAX_INDEX]);
+	}
+}
diff --git a/drivers/arm/gic/v3/gic600_multichip_private.h b/drivers/arm/gic/v3/gic600_multichip_private.h
new file mode 100644
index 0000000..414bd5b
--- /dev/null
+++ b/drivers/arm/gic/v3/gic600_multichip_private.h
@@ -0,0 +1,114 @@
+/*
+ * Copyright (c) 2019-2022, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef GIC600_MULTICHIP_PRIVATE_H
+#define GIC600_MULTICHIP_PRIVATE_H
+
+#include <drivers/arm/gic600_multichip.h>
+
+#include "gicv3_private.h"
+
+/* GIC600 GICD multichip related offsets */
+#define GICD_CHIPSR			U(0xC000)
+#define GICD_DCHIPR			U(0xC004)
+#define GICD_CHIPR			U(0xC008)
+
+/* GIC600 GICD multichip related masks */
+#define GICD_CHIPRx_PUP_BIT		BIT_64(1)
+#define GICD_CHIPRx_SOCKET_STATE	BIT_64(0)
+#define GICD_DCHIPR_PUP_BIT		BIT_32(0)
+#define GICD_CHIPSR_RTS_MASK		(BIT_32(4) | BIT_32(5))
+
+/* GIC600 GICD multichip related shifts */
+#define GICD_CHIPRx_ADDR_SHIFT		16
+#define GICD_CHIPSR_RTS_SHIFT		4
+#define GICD_DCHIPR_RT_OWNER_SHIFT	4
+
+/* Other shifts and masks remain the same between GIC-600 and GIC-700. */
+#define GIC_700_SPI_BLOCK_MIN_SHIFT	9
+#define GIC_700_SPI_BLOCKS_SHIFT	3
+#define GIC_600_SPI_BLOCK_MIN_SHIFT	10
+#define GIC_600_SPI_BLOCKS_SHIFT	5
+
+#define GICD_CHIPSR_RTS_STATE_DISCONNECTED	U(0)
+#define GICD_CHIPSR_RTS_STATE_UPDATING		U(1)
+#define GICD_CHIPSR_RTS_STATE_CONSISTENT	U(2)
+
+/* SPI interrupt id minimum and maximum range */
+#define GIC600_SPI_ID_MIN		32
+#define GIC600_SPI_ID_MAX		960
+
+#define GIC700_SPI_ID_MIN		32
+#define GIC700_SPI_ID_MAX		991
+#define GIC700_ESPI_ID_MIN		4096
+#define GIC700_ESPI_ID_MAX		5119
+
+/* Number of retries for PUP update */
+#define GICD_PUP_UPDATE_RETRIES		10000
+
+#define SPI_MIN_INDEX			0
+#define SPI_MAX_INDEX			1
+
+#define SPI_BLOCK_MIN_VALUE(spi_id_min) \
+			(((spi_id_min) - GIC600_SPI_ID_MIN) / \
+			GIC600_SPI_ID_MIN)
+#define SPI_BLOCKS_VALUE(spi_id_min, spi_id_max) \
+			(((spi_id_max) - (spi_id_min) + 1) / \
+			GIC600_SPI_ID_MIN)
+#define ESPI_BLOCK_MIN_VALUE(spi_id_min) \
+			(((spi_id_min) - GIC700_ESPI_ID_MIN + 1) / \
+			GIC700_SPI_ID_MIN)
+#define GICD_CHIPR_VALUE_GIC_700(chip_addr, spi_block_min, spi_blocks) \
+			(((chip_addr) << GICD_CHIPRx_ADDR_SHIFT) | \
+			((spi_block_min) << GIC_700_SPI_BLOCK_MIN_SHIFT) | \
+			((spi_blocks) << GIC_700_SPI_BLOCKS_SHIFT))
+#define GICD_CHIPR_VALUE_GIC_600(chip_addr, spi_block_min, spi_blocks) \
+			(((chip_addr) << GICD_CHIPRx_ADDR_SHIFT) | \
+			((spi_block_min) << GIC_600_SPI_BLOCK_MIN_SHIFT) | \
+			((spi_blocks) << GIC_600_SPI_BLOCKS_SHIFT))
+
+/*
+ * Multichip data assertion macros
+ */
+/* Set bits from 0 to ((spi_id_max + 1) / 32) */
+#define SPI_BLOCKS_TILL_MAX(spi_id_max) \
+			((1ULL << (((spi_id_max) + 1) >> 5)) - 1)
+/* Set bits from 0 to (spi_id_min / 32) */
+#define SPI_BLOCKS_TILL_MIN(spi_id_min)	((1 << ((spi_id_min) >> 5)) - 1)
+/* Set bits from (spi_id_min / 32) to ((spi_id_max + 1) / 32) */
+#define BLOCKS_OF_32(spi_id_min, spi_id_max) \
+					SPI_BLOCKS_TILL_MAX(spi_id_max) ^ \
+					SPI_BLOCKS_TILL_MIN(spi_id_min)
+
+/*******************************************************************************
+ * GIC-600 multichip operation related helper functions
+ ******************************************************************************/
+static inline uint32_t read_gicd_dchipr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_DCHIPR);
+}
+
+static inline uint64_t read_gicd_chipr_n(uintptr_t base, uint8_t n)
+{
+	return mmio_read_64(base + (GICD_CHIPR + (8U * n)));
+}
+
+static inline uint32_t read_gicd_chipsr(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_CHIPSR);
+}
+
+static inline void write_gicd_dchipr(uintptr_t base, uint32_t val)
+{
+	mmio_write_32(base + GICD_DCHIPR, val);
+}
+
+static inline void write_gicd_chipr_n(uintptr_t base, uint8_t n, uint64_t val)
+{
+	mmio_write_64(base + (GICD_CHIPR + (8U * n)), val);
+}
+
+#endif /* GIC600_MULTICHIP_PRIVATE_H */
diff --git a/drivers/arm/gic/v3/gic600ae_fmu.c b/drivers/arm/gic/v3/gic600ae_fmu.c
new file mode 100644
index 0000000..0262f48
--- /dev/null
+++ b/drivers/arm/gic/v3/gic600ae_fmu.c
@@ -0,0 +1,384 @@
+/*
+ * Copyright (c) 2021-2022, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+/*
+ * Driver for GIC-600AE Fault Management Unit
+ */
+
+#include <assert.h>
+#include <inttypes.h>
+
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <drivers/arm/gic600ae_fmu.h>
+#include <drivers/arm/gicv3.h>
+
+/* GIC-600 AE FMU specific register offsets */
+
+/* GIC-600 AE FMU specific macros */
+#define FMU_ERRIDR_NUM			U(44)
+#define FMU_ERRIDR_NUM_MASK		U(0xFFFF)
+
+/* Safety mechanisms for GICD block */
+static char *gicd_sm_info[] = {
+	"Reserved",
+	"GICD dual lockstep error",
+	"GICD AXI4 slave interface error",
+	"GICD-PPI AXI4-Stream interface error",
+	"GICD-ITS AXI4-Stream interface error",
+	"GICD-SPI-Collator AXI4-Stream interface error",
+	"GICD AXI4 master interface error",
+	"SPI RAM DED error",
+	"SGI RAM DED error",
+	"Reserved",
+	"LPI RAM DED error",
+	"GICD-remote-GICD AXI4-Stream interface error",
+	"GICD Q-Channel interface error",
+	"GICD P-Channel interface error",
+	"SPI RAM address decode error",
+	"SGI RAM address decode error",
+	"Reserved",
+	"LPI RAM address decode error",
+	"FMU dual lockstep error",
+	"FMU ping ACK error",
+	"FMU APB parity error",
+	"GICD-Wake AXI4-Stream interface error",
+	"GICD PageOffset or Chip ID error",
+	"MBIST REQ error",
+	"SPI RAM SEC error",
+	"SGI RAM SEC error",
+	"Reserved",
+	"LPI RAM SEC error",
+	"User custom SM0 error",
+	"User custom SM1 error",
+	"GICD-ITS Monolithic switch error",
+	"GICD-ITS Q-Channel interface error",
+	"GICD-ITS Monolithic interface error",
+	"GICD FMU ClkGate override"
+};
+
+/* Safety mechanisms for PPI block */
+static char *ppi_sm_info[] = {
+	"Reserved",
+	"PPI dual lockstep error",
+	"PPI-GICD AXI4-Stream interface error",
+	"PPI-CPU-IF AXI4-Stream interface error",
+	"PPI Q-Channel interface error",
+	"PPI RAM DED error",
+	"PPI RAM address decode error",
+	"PPI RAM SEC error",
+	"PPI User0 SM",
+	"PPI User1 SM",
+	"MBIST REQ error",
+	"PPI interrupt parity protection error",
+	"PPI FMU ClkGate override"
+};
+
+/* Safety mechanisms for ITS block */
+static char *its_sm_info[] = {
+	"Reserved",
+	"ITS dual lockstep error",
+	"ITS-GICD AXI4-Stream interface error",
+	"ITS AXI4 slave interface error",
+	"ITS AXI4 master interface error",
+	"ITS Q-Channel interface error",
+	"ITS RAM DED error",
+	"ITS RAM address decode error",
+	"Bypass ACE switch error",
+	"ITS RAM SEC error",
+	"ITS User0 SM",
+	"ITS User1 SM",
+	"ITS-GICD Monolithic interface error",
+	"MBIST REQ error",
+	"ITS FMU ClkGate override"
+};
+
+/* Safety mechanisms for SPI Collator block */
+static char *spicol_sm_info[] = {
+	"Reserved",
+	"SPI Collator dual lockstep error",
+	"SPI-Collator-GICD AXI4-Stream interface error",
+	"SPI Collator Q-Channel interface error",
+	"SPI Collator Q-Channel clock error",
+	"SPI interrupt parity error"
+};
+
+/* Safety mechanisms for Wake Request block */
+static char *wkrqst_sm_info[] = {
+	"Reserved",
+	"Wake dual lockstep error",
+	"Wake-GICD AXI4-Stream interface error"
+};
+
+/* Helper function to find detailed information for a specific IERR */
+static char __unused *ras_ierr_to_str(unsigned int blkid, unsigned int ierr)
+{
+	char *str = NULL;
+
+	/* Find the correct record */
+	switch (blkid) {
+	case FMU_BLK_GICD:
+		assert(ierr < ARRAY_SIZE(gicd_sm_info));
+		str = gicd_sm_info[ierr];
+		break;
+
+	case FMU_BLK_SPICOL:
+		assert(ierr < ARRAY_SIZE(spicol_sm_info));
+		str = spicol_sm_info[ierr];
+		break;
+
+	case FMU_BLK_WAKERQ:
+		assert(ierr < ARRAY_SIZE(wkrqst_sm_info));
+		str = wkrqst_sm_info[ierr];
+		break;
+
+	case FMU_BLK_ITS0...FMU_BLK_ITS7:
+		assert(ierr < ARRAY_SIZE(its_sm_info));
+		str = its_sm_info[ierr];
+		break;
+
+	case FMU_BLK_PPI0...FMU_BLK_PPI31:
+		assert(ierr < ARRAY_SIZE(ppi_sm_info));
+		str = ppi_sm_info[ierr];
+		break;
+
+	default:
+		assert(false);
+		break;
+	}
+
+	return str;
+}
+
+/*
+ * Probe for error in memory-mapped registers containing error records.
+ * Upon detecting an error, set probe data to the index of the record
+ * in error, and return 1; otherwise, return 0.
+ */
+int gic600_fmu_probe(uint64_t base, int *probe_data)
+{
+	uint64_t gsr;
+
+	assert(base != 0UL);
+
+	/*
+	 * Read ERR_GSR to find the error record 'M'
+	 */
+	gsr = gic_fmu_read_errgsr(base);
+	if (gsr == U(0)) {
+		return 0;
+	}
+
+	/* Return the index of the record in error */
+	if (probe_data != NULL) {
+		*probe_data = (int)__builtin_ctzll(gsr);
+	}
+
+	return 1;
+}
+
+/*
+ * The handler function to read RAS records and find the safety
+ * mechanism with the error.
+ */
+int gic600_fmu_ras_handler(uint64_t base, int probe_data)
+{
+	uint64_t errstatus;
+	unsigned int blkid = (unsigned int)probe_data, ierr, serr;
+
+	assert(base != 0UL);
+
+	/*
+	 * FMU_ERRGSR indicates the ID of the GIC
+	 * block that faulted.
+	 */
+	assert(blkid <= FMU_BLK_PPI31);
+
+	/*
+	 * Find more information by reading FMU_ERR<M>STATUS
+	 * register
+	 */
+	errstatus = gic_fmu_read_errstatus(base, blkid);
+
+	/*
+	 * If FMU_ERR<M>STATUS.V is set to 0, no RAS records
+	 * need to be scanned.
+	 */
+	if ((errstatus & FMU_ERRSTATUS_V_BIT) == U(0)) {
+		return 0;
+	}
+
+	/*
+	 * FMU_ERR<M>STATUS.IERR indicates which Safety Mechanism
+	 * reported the error.
+	 */
+	ierr = (errstatus >> FMU_ERRSTATUS_IERR_SHIFT) &
+			FMU_ERRSTATUS_IERR_MASK;
+
+	/*
+	 * FMU_ERR<M>STATUS.SERR indicates architecturally
+	 * defined primary error code.
+	 */
+	serr = errstatus & FMU_ERRSTATUS_SERR_MASK;
+
+	ERROR("**************************************\n");
+	ERROR("RAS %s Error detected by GIC600 AE FMU\n",
+		((errstatus & FMU_ERRSTATUS_UE_BIT) != 0U) ?
+			"Uncorrectable" : "Corrected");
+	ERROR("\tStatus = 0x%lx \n", errstatus);
+	ERROR("\tBlock ID = 0x%x\n", blkid);
+	ERROR("\tSafety Mechanism ID = 0x%x (%s)\n", ierr,
+		ras_ierr_to_str(blkid, ierr));
+	ERROR("\tArchitecturally defined primary error code = 0x%x\n",
+		serr);
+	ERROR("**************************************\n");
+
+	/* Clear FMU_ERR<M>STATUS */
+	gic_fmu_write_errstatus(base, probe_data, errstatus);
+
+	return 0;
+}
+
+/*
+ * Initialization sequence for the FMU
+ *
+ * 1. enable error detection for error records that are passed in the blk_present_mask
+ * 2. enable MBIST REQ and FMU Clk Gate override safety mechanisms for error records
+ *    that are present on the platform
+ *
+ * The platforms are expected to pass `errctlr_ce_en` and `errctlr_ue_en`.
+ */
+void gic600_fmu_init(uint64_t base, uint64_t blk_present_mask,
+		     bool errctlr_ce_en, bool errctlr_ue_en)
+{
+	unsigned int num_blk = gic_fmu_read_erridr(base) & FMU_ERRIDR_NUM_MASK;
+	uint64_t errctlr;
+	uint32_t smen;
+
+	INFO("GIC600-AE FMU supports %d error records\n", num_blk);
+
+	assert(num_blk == FMU_ERRIDR_NUM);
+
+	/* sanitize block present mask */
+	blk_present_mask &= FMU_BLK_PRESENT_MASK;
+
+	/* Enable error detection for all error records */
+	for (unsigned int i = 0U; i < num_blk; i++) {
+
+		/*
+		 * Disable all safety mechanisms for blocks that are not
+		 * present and skip the next steps.
+		 */
+		if ((blk_present_mask & BIT(i)) == 0U) {
+			gic_fmu_disable_all_sm_blkid(base, i);
+			continue;
+		}
+
+		/* Read the error record control register */
+		errctlr = gic_fmu_read_errctlr(base, i);
+
+		/* Enable error reporting and logging, if it is disabled */
+		if ((errctlr & FMU_ERRCTLR_ED_BIT) == 0U) {
+			errctlr |= FMU_ERRCTLR_ED_BIT;
+		}
+
+		/* Enable client provided ERRCTLR settings */
+		errctlr |= (errctlr_ce_en ? (FMU_ERRCTLR_CI_BIT | FMU_ERRCTLR_CE_EN_BIT) : 0);
+		errctlr |= (errctlr_ue_en ? FMU_ERRCTLR_UI_BIT : 0U);
+
+		gic_fmu_write_errctlr(base, i, errctlr);
+	}
+
+	/*
+	 * Enable MBIST REQ error and FMU CLK gate override safety mechanisms for
+	 * all blocks
+	 *
+	 * GICD, SMID 23 and SMID 33
+	 * PPI, SMID 10 and SMID 12
+	 * ITS, SMID 13 and SMID 14
+	 */
+	if ((blk_present_mask & BIT(FMU_BLK_GICD)) != 0U) {
+		smen = (GICD_MBIST_REQ_ERROR << FMU_SMEN_SMID_SHIFT) |
+			(FMU_BLK_GICD << FMU_SMEN_BLK_SHIFT) |
+			FMU_SMEN_EN_BIT;
+		gic_fmu_write_smen(base, smen);
+
+		smen = (GICD_FMU_CLKGATE_ERROR << FMU_SMEN_SMID_SHIFT) |
+			(FMU_BLK_GICD << FMU_SMEN_BLK_SHIFT) |
+			FMU_SMEN_EN_BIT;
+		gic_fmu_write_smen(base, smen);
+	}
+
+	for (unsigned int i = FMU_BLK_PPI0; i < FMU_BLK_PPI31; i++) {
+		if ((blk_present_mask & BIT(i)) != 0U) {
+			smen = (PPI_MBIST_REQ_ERROR << FMU_SMEN_SMID_SHIFT) |
+				(i << FMU_SMEN_BLK_SHIFT) |
+				FMU_SMEN_EN_BIT;
+			gic_fmu_write_smen(base, smen);
+
+			smen = (PPI_FMU_CLKGATE_ERROR << FMU_SMEN_SMID_SHIFT) |
+				(i << FMU_SMEN_BLK_SHIFT) |
+				FMU_SMEN_EN_BIT;
+			gic_fmu_write_smen(base, smen);
+		}
+	}
+
+	for (unsigned int i = FMU_BLK_ITS0; i < FMU_BLK_ITS7; i++) {
+		if ((blk_present_mask & BIT(i)) != 0U) {
+			smen = (ITS_MBIST_REQ_ERROR << FMU_SMEN_SMID_SHIFT) |
+				(i << FMU_SMEN_BLK_SHIFT) |
+				FMU_SMEN_EN_BIT;
+			gic_fmu_write_smen(base, smen);
+
+			smen = (ITS_FMU_CLKGATE_ERROR << FMU_SMEN_SMID_SHIFT) |
+				(i << FMU_SMEN_BLK_SHIFT) |
+				FMU_SMEN_EN_BIT;
+			gic_fmu_write_smen(base, smen);
+		}
+	}
+}
+
+/*
+ * This function enable the GICD background ping engine. The GICD sends ping
+ * messages to each remote GIC block, and expects a PING_ACK back within the
+ * specified timeout. Pings need to be enabled after programming the timeout
+ * value.
+ */
+void gic600_fmu_enable_ping(uint64_t base, uint64_t blk_present_mask,
+		unsigned int timeout_val, unsigned int interval_diff)
+{
+	/*
+	 * Populate the PING Mask to skip a specific block while generating
+	 * background ping messages and enable the ping mechanism.
+	 */
+	gic_fmu_write_pingmask(base, ~blk_present_mask);
+	gic_fmu_write_pingctlr(base, (interval_diff << FMU_PINGCTLR_INTDIFF_SHIFT) |
+		(timeout_val << FMU_PINGCTLR_TIMEOUTVAL_SHIFT) | FMU_PINGCTLR_EN_BIT);
+}
+
+/* Print the safety mechanism description for a given block */
+void gic600_fmu_print_sm_info(uint64_t base, unsigned int blk, unsigned int smid)
+{
+	if (blk == FMU_BLK_GICD && smid <= FMU_SMID_GICD_MAX) {
+		INFO("GICD, SMID %d: %s\n", smid, gicd_sm_info[smid]);
+	}
+
+	if (blk == FMU_BLK_SPICOL && smid <= FMU_SMID_SPICOL_MAX) {
+		INFO("SPI Collator, SMID %d: %s\n", smid, spicol_sm_info[smid]);
+	}
+
+	if (blk == FMU_BLK_WAKERQ && (smid <= FMU_SMID_WAKERQ_MAX)) {
+		INFO("Wake Request, SMID %d: %s\n", smid, wkrqst_sm_info[smid]);
+	}
+
+	if (((blk >= FMU_BLK_ITS0) && (blk <= FMU_BLK_ITS7)) && (smid <= FMU_SMID_ITS_MAX)) {
+		INFO("ITS, SMID %d: %s\n", smid, its_sm_info[smid]);
+	}
+
+	if (((blk >= FMU_BLK_PPI0) && (blk <= FMU_BLK_PPI31)) && (smid <= FMU_SMID_PPI_MAX)) {
+		INFO("PPI, SMID %d: %s\n", smid, ppi_sm_info[smid]);
+	}
+}
diff --git a/drivers/arm/gic/v3/gic600ae_fmu_helpers.c b/drivers/arm/gic/v3/gic600ae_fmu_helpers.c
new file mode 100644
index 0000000..09806dc
--- /dev/null
+++ b/drivers/arm/gic/v3/gic600ae_fmu_helpers.c
@@ -0,0 +1,304 @@
+/*
+ * Copyright (c) 2021-2022, NVIDIA Corporation. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <drivers/arm/gic600ae_fmu.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+#define GICFMU_IDLE_TIMEOUT_US		U(2000000)
+
+/* Macro to write 32-bit FMU registers */
+#define GIC_FMU_WRITE_32(base, reg, val) \
+	do { \
+		/* \
+		 * This register receives the unlock key that is required for \
+		 * writes to FMU registers to be successful. \
+		 */ \
+		mmio_write_32(base + GICFMU_KEY, 0xBE); \
+		/* Perform the actual write */ \
+		mmio_write_32((base) + (reg), (val)); \
+	} while (false)
+
+/* Macro to write 64-bit FMU registers */
+#define GIC_FMU_WRITE_64(base, reg, n, val) \
+	do { \
+		/* \
+		 * This register receives the unlock key that is required for \
+		 * writes to FMU registers to be successful. \
+		 */ \
+		mmio_write_32(base + GICFMU_KEY, 0xBE); \
+		/* \
+		 * APB bus is 32-bit wide; so split the 64-bit write into \
+		 * two 32-bit writes \
+		 */ \
+		mmio_write_32((base) + reg##_LO + (n * 64), (val)); \
+		mmio_write_32((base) + reg##_HI + (n * 64), (val)); \
+	} while (false)
+
+/* Helper function to wait until FMU is ready to accept the next command */
+static void wait_until_fmu_is_idle(uintptr_t base)
+{
+	uint32_t timeout_count = GICFMU_IDLE_TIMEOUT_US;
+	uint64_t status;
+
+	/* wait until status is 'busy' */
+	do {
+		status = (gic_fmu_read_status(base) & BIT(0));
+
+		if (timeout_count-- == 0U) {
+			ERROR("GIC600 AE FMU is not responding\n");
+			panic();
+		}
+
+		udelay(1U);
+
+	} while (status == U(0));
+}
+
+#define GIC_FMU_WRITE_ON_IDLE_32(base, reg, val) \
+	do { \
+		/* Wait until FMU is ready */ \
+		wait_until_fmu_is_idle(base); \
+		/* Actual register write */ \
+		GIC_FMU_WRITE_32(base, reg, val); \
+		/* Wait until FMU is ready */ \
+		wait_until_fmu_is_idle(base); \
+	} while (false)
+
+#define GIC_FMU_WRITE_ON_IDLE_64(base, reg, n, val) \
+	do { \
+		/* Wait until FMU is ready */ \
+		wait_until_fmu_is_idle(base); \
+		/* Actual register write */ \
+		GIC_FMU_WRITE_64(base, reg, n, val); \
+		/* Wait until FMU is ready */ \
+		wait_until_fmu_is_idle(base); \
+	} while (false)
+
+/*******************************************************************************
+ * GIC FMU functions for accessing the Fault Management Unit registers
+ ******************************************************************************/
+
+/*
+ * Accessors to read the Error Record Feature Register bits corresponding
+ * to an error record 'n'
+ */
+uint64_t gic_fmu_read_errfr(uintptr_t base, unsigned int n)
+{
+	/*
+	 * APB bus is 32-bit wide; so split the 64-bit read into
+	 * two 32-bit reads
+	 */
+	uint64_t reg_val = (uint64_t)mmio_read_32(base + GICFMU_ERRFR_LO + n * 64U);
+
+	reg_val |= ((uint64_t)mmio_read_32(base + GICFMU_ERRFR_HI + n * 64U) << 32);
+	return reg_val;
+}
+
+/*
+ * Accessors to read the Error Record Control Register bits corresponding
+ * to an error record 'n'
+ */
+uint64_t gic_fmu_read_errctlr(uintptr_t base, unsigned int n)
+{
+	/*
+	 * APB bus is 32-bit wide; so split the 64-bit read into
+	 * two 32-bit reads
+	 */
+	uint64_t reg_val = (uint64_t)mmio_read_32(base + GICFMU_ERRCTLR_LO + n * 64U);
+
+	reg_val |= ((uint64_t)mmio_read_32(base + GICFMU_ERRCTLR_HI + n * 64U) << 32);
+	return reg_val;
+}
+
+/*
+ * Accessors to read the Error Record Primary Status Register bits
+ * corresponding to an error record 'n'
+ */
+uint64_t gic_fmu_read_errstatus(uintptr_t base, unsigned int n)
+{
+	/*
+	 * APB bus is 32-bit wide; so split the 64-bit read into
+	 * two 32-bit reads
+	 */
+	uint64_t reg_val = (uint64_t)mmio_read_32(base + GICFMU_ERRSTATUS_LO + n * 64U);
+
+	reg_val |= ((uint64_t)mmio_read_32(base + GICFMU_ERRSTATUS_HI + n * 64U) << 32);
+	return reg_val;
+}
+
+/*
+ * Accessors to read the Error Group Status Register
+ */
+uint64_t gic_fmu_read_errgsr(uintptr_t base)
+{
+	/*
+	 * APB bus is 32-bit wide; so split the 64-bit read into
+	 * two 32-bit reads
+	 */
+	uint64_t reg_val = (uint64_t)mmio_read_32(base + GICFMU_ERRGSR_LO);
+
+	reg_val |= ((uint64_t)mmio_read_32(base + GICFMU_ERRGSR_HI) << 32);
+	return reg_val;
+}
+
+/*
+ * Accessors to read the Ping Control Register
+ */
+uint32_t gic_fmu_read_pingctlr(uintptr_t base)
+{
+	return mmio_read_32(base + GICFMU_PINGCTLR);
+}
+
+/*
+ * Accessors to read the Ping Now Register
+ */
+uint32_t gic_fmu_read_pingnow(uintptr_t base)
+{
+	return mmio_read_32(base + GICFMU_PINGNOW);
+}
+
+/*
+ * Accessors to read the Ping Mask Register
+ */
+uint64_t gic_fmu_read_pingmask(uintptr_t base)
+{
+	/*
+	 * APB bus is 32-bit wide; so split the 64-bit read into
+	 * two 32-bit reads
+	 */
+	uint64_t reg_val = (uint64_t)mmio_read_32(base + GICFMU_PINGMASK_LO);
+
+	reg_val |= ((uint64_t)mmio_read_32(base + GICFMU_PINGMASK_HI) << 32);
+	return reg_val;
+}
+
+/*
+ * Accessors to read the FMU Status Register
+ */
+uint32_t gic_fmu_read_status(uintptr_t base)
+{
+	return mmio_read_32(base + GICFMU_STATUS);
+}
+
+/*
+ * Accessors to read the Error Record ID Register
+ */
+uint32_t gic_fmu_read_erridr(uintptr_t base)
+{
+	return mmio_read_32(base + GICFMU_ERRIDR);
+}
+
+/*
+ * Accessors to write a 64 bit value to the Error Record Control Register
+ */
+void gic_fmu_write_errctlr(uintptr_t base, unsigned int n, uint64_t val)
+{
+	GIC_FMU_WRITE_64(base, GICFMU_ERRCTLR, n, val);
+}
+
+/*
+ * Accessors to write a 64 bit value to the Error Record Primary Status
+ * Register
+ */
+void gic_fmu_write_errstatus(uintptr_t base, unsigned int n, uint64_t val)
+{
+	/* Wait until FMU is ready before writing */
+	GIC_FMU_WRITE_ON_IDLE_64(base, GICFMU_ERRSTATUS, n, val);
+}
+
+/*
+ * Accessors to write a 32 bit value to the Ping Control Register
+ */
+void gic_fmu_write_pingctlr(uintptr_t base, uint32_t val)
+{
+	GIC_FMU_WRITE_32(base, GICFMU_PINGCTLR, val);
+}
+
+/*
+ * Accessors to write a 32 bit value to the Ping Now Register
+ */
+void gic_fmu_write_pingnow(uintptr_t base, uint32_t val)
+{
+	/* Wait until FMU is ready before writing */
+	GIC_FMU_WRITE_ON_IDLE_32(base, GICFMU_PINGNOW, val);
+}
+
+/*
+ * Accessors to write a 32 bit value to the Safety Mechanism Enable Register
+ */
+void gic_fmu_write_smen(uintptr_t base, uint32_t val)
+{
+	/* Wait until FMU is ready before writing */
+	GIC_FMU_WRITE_ON_IDLE_32(base, GICFMU_SMEN, val);
+}
+
+/*
+ * Accessors to write a 32 bit value to the Safety Mechanism Inject Error
+ * Register
+ */
+void gic_fmu_write_sminjerr(uintptr_t base, uint32_t val)
+{
+	/* Wait until FMU is ready before writing */
+	GIC_FMU_WRITE_ON_IDLE_32(base, GICFMU_SMINJERR, val);
+}
+
+/*
+ * Accessors to write a 64 bit value to the Ping Mask Register
+ */
+void gic_fmu_write_pingmask(uintptr_t base, uint64_t val)
+{
+	GIC_FMU_WRITE_64(base, GICFMU_PINGMASK, 0, val);
+}
+
+/*
+ * Helper function to disable all safety mechanisms for a given block
+ */
+void gic_fmu_disable_all_sm_blkid(uintptr_t base, unsigned int blkid)
+{
+	uint32_t smen, max_smid = U(0);
+
+	/* Sanity check block ID */
+	assert((blkid >= FMU_BLK_GICD) && (blkid <= FMU_BLK_PPI31));
+
+	/* Find the max safety mechanism ID for the block */
+	switch (blkid) {
+	case FMU_BLK_GICD:
+		max_smid = FMU_SMID_GICD_MAX;
+		break;
+
+	case FMU_BLK_SPICOL:
+		max_smid = FMU_SMID_SPICOL_MAX;
+		break;
+
+	case FMU_BLK_WAKERQ:
+		max_smid = FMU_SMID_WAKERQ_MAX;
+		break;
+
+	case FMU_BLK_ITS0...FMU_BLK_ITS7:
+		max_smid = FMU_SMID_ITS_MAX;
+		break;
+
+	case FMU_BLK_PPI0...FMU_BLK_PPI31:
+		max_smid = FMU_SMID_PPI_MAX;
+		break;
+
+	default:
+		assert(false);
+		break;
+	}
+
+	/* Disable all Safety Mechanisms for a given block id */
+	for (unsigned int i = 0U; i < max_smid; i++) {
+		smen = (blkid << FMU_SMEN_BLK_SHIFT) | (i << FMU_SMEN_SMID_SHIFT);
+		gic_fmu_write_smen(base, smen);
+	}
+}
diff --git a/drivers/arm/gic/v3/gicdv3_helpers.c b/drivers/arm/gic/v3/gicdv3_helpers.c
new file mode 100644
index 0000000..987be69
--- /dev/null
+++ b/drivers/arm/gic/v3/gicdv3_helpers.c
@@ -0,0 +1,244 @@
+/*
+ * Copyright (c) 2015-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include "gicv3_private.h"
+
+/*******************************************************************************
+ * GIC Distributor functions for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+
+/*
+ * Accessors to set the bits corresponding to interrupt ID
+ * in GIC Distributor ICFGR and ICFGRE.
+ */
+void gicd_set_icfgr(uintptr_t base, unsigned int id, unsigned int cfg)
+{
+	/* Interrupt configuration is a 2-bit field */
+	unsigned int bit_shift = BIT_NUM(ICFG, id) << 1U;
+
+	/* Clear the field, and insert required configuration */
+	mmio_clrsetbits_32(base + GICD_OFFSET(ICFG, id),
+				(uint32_t)GIC_CFG_MASK << bit_shift,
+				(cfg & GIC_CFG_MASK) << bit_shift);
+}
+
+/*
+ * Accessors to get/set/clear the bit corresponding to interrupt ID
+ * in GIC Distributor IGROUPR and IGROUPRE.
+ */
+unsigned int gicd_get_igroupr(uintptr_t base, unsigned int id)
+{
+	return GICD_GET_BIT(IGROUP, base, id);
+}
+
+void gicd_set_igroupr(uintptr_t base, unsigned int id)
+{
+	GICD_SET_BIT(IGROUP, base, id);
+}
+
+void gicd_clr_igroupr(uintptr_t base, unsigned int id)
+{
+	GICD_CLR_BIT(IGROUP, base, id);
+}
+
+/*
+ * Accessors to get/set/clear the bit corresponding to interrupt ID
+ * in GIC Distributor IGRPMODR and IGRPMODRE.
+ */
+unsigned int gicd_get_igrpmodr(uintptr_t base, unsigned int id)
+{
+	return GICD_GET_BIT(IGRPMOD, base, id);
+}
+
+void gicd_set_igrpmodr(uintptr_t base, unsigned int id)
+{
+	GICD_SET_BIT(IGRPMOD, base, id);
+}
+
+void gicd_clr_igrpmodr(uintptr_t base, unsigned int id)
+{
+	GICD_CLR_BIT(IGRPMOD, base, id);
+}
+
+/*
+ * Accessors to set the bit corresponding to interrupt ID
+ * in GIC Distributor ICENABLER and ICENABLERE.
+ */
+void gicd_set_icenabler(uintptr_t base, unsigned int id)
+{
+	GICD_WRITE_BIT(ICENABLE, base, id);
+}
+
+/*
+ * Accessors to set the bit corresponding to interrupt ID
+ * in GIC Distributor ICPENDR and ICPENDRE.
+ */
+void gicd_set_icpendr(uintptr_t base, unsigned int id)
+{
+	GICD_WRITE_BIT(ICPEND, base, id);
+}
+
+/*
+ * Accessors to get/set the bit corresponding to interrupt ID
+ * in GIC Distributor ISACTIVER and ISACTIVERE.
+ */
+unsigned int gicd_get_isactiver(uintptr_t base, unsigned int id)
+{
+	return GICD_GET_BIT(ISACTIVE, base, id);
+}
+
+void gicd_set_isactiver(uintptr_t base, unsigned int id)
+{
+	GICD_WRITE_BIT(ISACTIVE, base, id);
+}
+
+/*
+ * Accessors to set the bit corresponding to interrupt ID
+ * in GIC Distributor ISENABLER and ISENABLERE.
+ */
+void gicd_set_isenabler(uintptr_t base, unsigned int id)
+{
+	GICD_WRITE_BIT(ISENABLE, base, id);
+}
+
+/*
+ * Accessors to set the bit corresponding to interrupt ID
+ * in GIC Distributor ISPENDR and ISPENDRE.
+ */
+void gicd_set_ispendr(uintptr_t base, unsigned int id)
+{
+	GICD_WRITE_BIT(ISPEND, base, id);
+}
+
+/*
+ * Accessors to set the bit corresponding to interrupt ID
+ * in GIC Distributor IPRIORITYR and IPRIORITYRE.
+ */
+void gicd_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	GICD_WRITE_8(IPRIORITY, base, id, (uint8_t)(pri & GIC_PRI_MASK));
+}
+
+/*******************************************************************************
+ * GIC Distributor interface accessors for reading/writing entire registers
+ ******************************************************************************/
+
+/*
+ * Accessors to read/write the GIC Distributor ICGFR and ICGFRE
+ * corresponding to the interrupt ID, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_icfgr(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(ICFG, base, id);
+}
+
+void gicd_write_icfgr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(ICFG, base, id, val);
+}
+
+/*
+ * Accessors to read/write the GIC Distributor IGROUPR and IGROUPRE
+ * corresponding to the interrupt ID, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_igroupr(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(IGROUP, base, id);
+}
+
+void gicd_write_igroupr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(IGROUP, base, id, val);
+}
+
+/*
+ * Accessors to read/write the GIC Distributor IGRPMODR and IGRPMODRE
+ * corresponding to the interrupt ID, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_igrpmodr(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(IGRPMOD, base, id);
+}
+
+void gicd_write_igrpmodr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(IGRPMOD, base, id, val);
+}
+
+/*
+ * Accessors to read/write the GIC Distributor IPRIORITYR and IPRIORITYRE
+ * corresponding to the interrupt ID, 4 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ipriorityr(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(IPRIORITY, base, id);
+}
+
+void gicd_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(IPRIORITY, base, id, val);
+}
+
+/*
+ * Accessors to read/write the GIC Distributor ISACTIVER and ISACTIVERE
+ * corresponding to the interrupt ID, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_isactiver(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(ISACTIVE, base, id);
+}
+
+void gicd_write_isactiver(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(ISACTIVE, base, id, val);
+}
+
+/*
+ * Accessors to read/write the GIC Distributor ISENABLER and ISENABLERE
+ * corresponding to the interrupt ID, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_isenabler(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(ISENABLE, base, id);
+}
+
+void gicd_write_isenabler(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(ISENABLE, base, id, val);
+}
+
+/*
+ * Accessors to read/write the GIC Distributor ISPENDR and ISPENDRE
+ * corresponding to the interrupt ID, 32 interrupt IDs at a time.
+ */
+unsigned int gicd_read_ispendr(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(ISPEND, base, id);
+}
+
+void gicd_write_ispendr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(ISPEND, base, id, val);
+}
+
+/*
+ * Accessors to read/write the GIC Distributor NSACR and NSACRE
+ * corresponding to the interrupt ID, 16 interrupt IDs at a time.
+ */
+unsigned int gicd_read_nsacr(uintptr_t base, unsigned int id)
+{
+	return GICD_READ(NSAC, base, id);
+}
+
+void gicd_write_nsacr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICD_WRITE(NSAC, base, id, val);
+}
diff --git a/drivers/arm/gic/v3/gicrv3_helpers.c b/drivers/arm/gic/v3/gicrv3_helpers.c
new file mode 100644
index 0000000..3004054
--- /dev/null
+++ b/drivers/arm/gic/v3/gicrv3_helpers.c
@@ -0,0 +1,139 @@
+/*
+ * Copyright (c) 2015-2020, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/interrupt_props.h>
+#include <drivers/arm/gicv3.h>
+#include "gicv3_private.h"
+
+/*******************************************************************************
+ * GIC Redistributor functions
+ * Note: The raw register values correspond to multiple interrupt `id`s and
+ * the number of interrupt `id`s involved depends on the register accessed.
+ ******************************************************************************/
+
+/*
+ * Accessors to read/write the GIC Redistributor IPRIORITYR and IPRIORITYRE
+ * register corresponding to the interrupt `id`, 4 interrupts IDs at a time.
+ */
+unsigned int gicr_read_ipriorityr(uintptr_t base, unsigned int id)
+{
+	return GICR_READ(IPRIORITY, base, id);
+}
+
+void gicr_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val)
+{
+	GICR_WRITE(IPRIORITY, base, id, val);
+}
+
+/*
+ * Accessor to set the byte corresponding to interrupt `id`
+ * in GIC Redistributor IPRIORITYR and IPRIORITYRE.
+ */
+void gicr_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri)
+{
+	GICR_WRITE_8(IPRIORITY, base, id, (uint8_t)(pri & GIC_PRI_MASK));
+}
+
+/*
+ * Accessors to get/set/clear the bit corresponding to interrupt `id`
+ * from GIC Redistributor IGROUPR0 and IGROUPRE
+ */
+unsigned int gicr_get_igroupr(uintptr_t base, unsigned int id)
+{
+	return GICR_GET_BIT(IGROUP, base, id);
+}
+
+void gicr_set_igroupr(uintptr_t base, unsigned int id)
+{
+	GICR_SET_BIT(IGROUP, base, id);
+}
+
+void gicr_clr_igroupr(uintptr_t base, unsigned int id)
+{
+	GICR_CLR_BIT(IGROUP, base, id);
+}
+
+/*
+ * Accessors to get/set/clear the bit corresponding to interrupt `id`
+ * from GIC Redistributor IGRPMODR0 and IGRPMODRE
+ */
+unsigned int gicr_get_igrpmodr(uintptr_t base, unsigned int id)
+{
+	return GICR_GET_BIT(IGRPMOD, base, id);
+}
+
+void gicr_set_igrpmodr(uintptr_t base, unsigned int id)
+{
+	GICR_SET_BIT(IGRPMOD, base, id);
+}
+
+void gicr_clr_igrpmodr(uintptr_t base, unsigned int id)
+{
+	GICR_CLR_BIT(IGRPMOD, base, id);
+}
+
+/*
+ * Accessor to write the bit corresponding to interrupt `id`
+ * in GIC Redistributor ISENABLER0 and ISENABLERE
+ */
+void gicr_set_isenabler(uintptr_t base, unsigned int id)
+{
+	GICR_WRITE_BIT(ISENABLE, base, id);
+}
+
+/*
+ * Accessor to write the bit corresponding to interrupt `id`
+ * in GIC Redistributor ICENABLER0 and ICENABLERE
+ */
+void gicr_set_icenabler(uintptr_t base, unsigned int id)
+{
+	GICR_WRITE_BIT(ICENABLE, base, id);
+}
+
+/*
+ * Accessor to get the bit corresponding to interrupt `id`
+ * in GIC Redistributor ISACTIVER0 and ISACTIVERE
+ */
+unsigned int gicr_get_isactiver(uintptr_t base, unsigned int id)
+{
+	return	GICR_GET_BIT(ISACTIVE, base, id);
+}
+
+/*
+ * Accessor to clear the bit corresponding to interrupt `id`
+ * in GIC Redistributor ICPENDR0 and ICPENDRE
+ */
+void gicr_set_icpendr(uintptr_t base, unsigned int id)
+{
+	GICR_WRITE_BIT(ICPEND, base, id);
+}
+
+/*
+ * Accessor to write the bit corresponding to interrupt `id`
+ * in GIC Redistributor ISPENDR0 and ISPENDRE
+ */
+void gicr_set_ispendr(uintptr_t base, unsigned int id)
+{
+	GICR_WRITE_BIT(ISPEND, base, id);
+}
+
+/*
+ * Accessor to set the bit fields corresponding to interrupt `id`
+ * in GIC Redistributor ICFGR0, ICFGR1 and ICFGRE
+ */
+void gicr_set_icfgr(uintptr_t base, unsigned int id, unsigned int cfg)
+{
+	/* Interrupt configuration is a 2-bit field */
+	unsigned int bit_shift = BIT_NUM(ICFG, id) << 1U;
+
+	/* Clear the field, and insert required configuration */
+	mmio_clrsetbits_32(base + GICR_OFFSET(ICFG, id),
+				(uint32_t)GIC_CFG_MASK << bit_shift,
+				(cfg & GIC_CFG_MASK) << bit_shift);
+}
diff --git a/drivers/arm/gic/v3/gicv3.mk b/drivers/arm/gic/v3/gicv3.mk
new file mode 100644
index 0000000..1d20ff3
--- /dev/null
+++ b/drivers/arm/gic/v3/gicv3.mk
@@ -0,0 +1,54 @@
+#
+# Copyright (c) 2013-2022, Arm Limited and Contributors. All rights reserved.
+# Copyright (c) 2021, NVIDIA Corporation. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+# Default configuration values
+GICV3_SUPPORT_GIC600		?=	0
+GICV3_SUPPORT_GIC600AE_FMU	?=	0
+GICV3_IMPL_GIC600_MULTICHIP	?=	0
+GICV3_OVERRIDE_DISTIF_PWR_OPS	?=	0
+GIC_ENABLE_V4_EXTN		?=	0
+GIC_EXT_INTID			?=	0
+GIC600_ERRATA_WA_2384374	?=	${GICV3_SUPPORT_GIC600}
+
+GICV3_SOURCES	+=	drivers/arm/gic/v3/gicv3_main.c		\
+			drivers/arm/gic/v3/gicv3_helpers.c	\
+			drivers/arm/gic/v3/gicdv3_helpers.c	\
+			drivers/arm/gic/v3/gicrv3_helpers.c
+
+ifeq (${GICV3_SUPPORT_GIC600AE_FMU}, 1)
+GICV3_SOURCES	+=	drivers/arm/gic/v3/gic600ae_fmu.c	\
+			drivers/arm/gic/v3/gic600ae_fmu_helpers.c
+endif
+
+ifeq (${GICV3_OVERRIDE_DISTIF_PWR_OPS}, 0)
+GICV3_SOURCES	+=	drivers/arm/gic/v3/arm_gicv3_common.c
+endif
+
+GICV3_SOURCES	+=	drivers/arm/gic/v3/gic-x00.c
+ifeq (${GICV3_IMPL_GIC600_MULTICHIP}, 1)
+GICV3_SOURCES	+=	drivers/arm/gic/v3/gic600_multichip.c
+endif
+
+# Set GIC-600 support
+$(eval $(call assert_boolean,GICV3_SUPPORT_GIC600))
+$(eval $(call add_define,GICV3_SUPPORT_GIC600))
+
+# Set GIC-600AE FMU support
+$(eval $(call assert_boolean,GICV3_SUPPORT_GIC600AE_FMU))
+$(eval $(call add_define,GICV3_SUPPORT_GIC600AE_FMU))
+
+# Set GICv4 extension
+$(eval $(call assert_boolean,GIC_ENABLE_V4_EXTN))
+$(eval $(call add_define,GIC_ENABLE_V4_EXTN))
+
+# Set support for extended PPI and SPI range
+$(eval $(call assert_boolean,GIC_EXT_INTID))
+$(eval $(call add_define,GIC_EXT_INTID))
+
+# Set errata workaround for GIC600/GIC600AE
+$(eval $(call assert_boolean,GIC600_ERRATA_WA_2384374))
+$(eval $(call add_define,GIC600_ERRATA_WA_2384374))
diff --git a/drivers/arm/gic/v3/gicv3_helpers.c b/drivers/arm/gic/v3/gicv3_helpers.c
new file mode 100644
index 0000000..446d0ad
--- /dev/null
+++ b/drivers/arm/gic/v3/gicv3_helpers.c
@@ -0,0 +1,443 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/interrupt_props.h>
+#include <drivers/arm/gic_common.h>
+
+#include <platform_def.h>
+
+#include "../common/gic_common_private.h"
+#include "gicv3_private.h"
+
+/******************************************************************************
+ * This function marks the core as awake in the re-distributor and
+ * ensures that the interface is active.
+ *****************************************************************************/
+void gicv3_rdistif_mark_core_awake(uintptr_t gicr_base)
+{
+	/*
+	 * The WAKER_PS_BIT should be changed to 0
+	 * only when WAKER_CA_BIT is 1.
+	 */
+	assert((gicr_read_waker(gicr_base) & WAKER_CA_BIT) != 0U);
+
+	/* Mark the connected core as awake */
+	gicr_write_waker(gicr_base, gicr_read_waker(gicr_base) & ~WAKER_PS_BIT);
+
+	/* Wait till the WAKER_CA_BIT changes to 0 */
+	while ((gicr_read_waker(gicr_base) & WAKER_CA_BIT) != 0U) {
+	}
+}
+
+/******************************************************************************
+ * This function marks the core as asleep in the re-distributor and ensures
+ * that the interface is quiescent.
+ *****************************************************************************/
+void gicv3_rdistif_mark_core_asleep(uintptr_t gicr_base)
+{
+	/* Mark the connected core as asleep */
+	gicr_write_waker(gicr_base, gicr_read_waker(gicr_base) | WAKER_PS_BIT);
+
+	/* Wait till the WAKER_CA_BIT changes to 1 */
+	while ((gicr_read_waker(gicr_base) & WAKER_CA_BIT) == 0U) {
+	}
+}
+
+/*******************************************************************************
+ * This function probes the Redistributor frames when the driver is initialised
+ * and saves their base addresses. These base addresses are used later to
+ * initialise each Redistributor interface.
+ ******************************************************************************/
+void gicv3_rdistif_base_addrs_probe(uintptr_t *rdistif_base_addrs,
+					unsigned int rdistif_num,
+					uintptr_t gicr_base,
+					mpidr_hash_fn mpidr_to_core_pos)
+{
+	u_register_t mpidr;
+	unsigned int proc_num;
+	uint64_t typer_val;
+	uintptr_t rdistif_base = gicr_base;
+
+	assert(rdistif_base_addrs != NULL);
+
+	/*
+	 * Iterate over the Redistributor frames. Store the base address of each
+	 * frame in the platform provided array. Use the "Processor Number"
+	 * field to index into the array if the platform has not provided a hash
+	 * function to convert an MPIDR (obtained from the "Affinity Value"
+	 * field into a linear index.
+	 */
+	do {
+		typer_val = gicr_read_typer(rdistif_base);
+		if (mpidr_to_core_pos != NULL) {
+			mpidr = mpidr_from_gicr_typer(typer_val);
+			proc_num = mpidr_to_core_pos(mpidr);
+		} else {
+			proc_num = (typer_val >> TYPER_PROC_NUM_SHIFT) &
+				TYPER_PROC_NUM_MASK;
+		}
+
+		if (proc_num < rdistif_num) {
+			rdistif_base_addrs[proc_num] = rdistif_base;
+		}
+		rdistif_base += gicv3_redist_size(typer_val);
+	} while ((typer_val & TYPER_LAST_BIT) == 0U);
+}
+
+/*******************************************************************************
+ * Helper function to get the maximum SPI INTID + 1.
+ ******************************************************************************/
+unsigned int gicv3_get_spi_limit(uintptr_t gicd_base)
+{
+	unsigned int spi_limit;
+	unsigned int typer_reg = gicd_read_typer(gicd_base);
+
+	/* (maximum SPI INTID + 1) is equal to 32 * (GICD_TYPER.ITLinesNumber+1) */
+	spi_limit = ((typer_reg & TYPER_IT_LINES_NO_MASK) + 1U) << 5;
+
+	/* Filter out special INTIDs 1020-1023 */
+	if (spi_limit > (MAX_SPI_ID + 1U)) {
+		return MAX_SPI_ID + 1U;
+	}
+
+	return spi_limit;
+}
+
+#if GIC_EXT_INTID
+/*******************************************************************************
+ * Helper function to get the maximum ESPI INTID + 1.
+ ******************************************************************************/
+unsigned int gicv3_get_espi_limit(uintptr_t gicd_base)
+{
+	unsigned int typer_reg = gicd_read_typer(gicd_base);
+
+	/* Check if extended SPI range is implemented */
+	if ((typer_reg & TYPER_ESPI) != 0U) {
+		/*
+		 * (maximum ESPI INTID + 1) is equal to
+		 * 32 * (GICD_TYPER.ESPI_range + 1) + 4096
+		 */
+		return ((((typer_reg >> TYPER_ESPI_RANGE_SHIFT) &
+			TYPER_ESPI_RANGE_MASK) + 1U) << 5) + MIN_ESPI_ID;
+	}
+
+	return 0U;
+}
+#endif /* GIC_EXT_INTID */
+
+/*******************************************************************************
+ * Helper function to configure the default attributes of (E)SPIs.
+ ******************************************************************************/
+void gicv3_spis_config_defaults(uintptr_t gicd_base)
+{
+	unsigned int i, num_ints;
+#if GIC_EXT_INTID
+	unsigned int num_eints;
+#endif
+
+	num_ints = gicv3_get_spi_limit(gicd_base);
+	INFO("Maximum SPI INTID supported: %u\n", num_ints - 1);
+
+	/* Treat all (E)SPIs as G1NS by default. We do 32 at a time. */
+	for (i = MIN_SPI_ID; i < num_ints; i += (1U << IGROUPR_SHIFT)) {
+		gicd_write_igroupr(gicd_base, i, ~0U);
+	}
+
+#if GIC_EXT_INTID
+	num_eints = gicv3_get_espi_limit(gicd_base);
+	if (num_eints != 0U) {
+		INFO("Maximum ESPI INTID supported: %u\n", num_eints - 1);
+
+		for (i = MIN_ESPI_ID; i < num_eints;
+					i += (1U << IGROUPR_SHIFT)) {
+			gicd_write_igroupr(gicd_base, i, ~0U);
+		}
+	} else {
+		INFO("ESPI range is not implemented.\n");
+	}
+#endif
+
+	/* Setup the default (E)SPI priorities doing four at a time */
+	for (i = MIN_SPI_ID; i < num_ints; i += (1U << IPRIORITYR_SHIFT)) {
+		gicd_write_ipriorityr(gicd_base, i, GICD_IPRIORITYR_DEF_VAL);
+	}
+
+#if GIC_EXT_INTID
+	for (i = MIN_ESPI_ID; i < num_eints;
+					i += (1U << IPRIORITYR_SHIFT)) {
+		gicd_write_ipriorityr(gicd_base, i, GICD_IPRIORITYR_DEF_VAL);
+	}
+#endif
+	/*
+	 * Treat all (E)SPIs as level triggered by default, write 16 at a time
+	 */
+	for (i = MIN_SPI_ID; i < num_ints; i += (1U << ICFGR_SHIFT)) {
+		gicd_write_icfgr(gicd_base, i, 0U);
+	}
+
+#if GIC_EXT_INTID
+	for (i = MIN_ESPI_ID; i < num_eints; i += (1U << ICFGR_SHIFT)) {
+		gicd_write_icfgr(gicd_base, i, 0U);
+	}
+#endif
+}
+
+/*******************************************************************************
+ * Helper function to configure properties of secure (E)SPIs
+ ******************************************************************************/
+unsigned int gicv3_secure_spis_config_props(uintptr_t gicd_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num)
+{
+	unsigned int i;
+	const interrupt_prop_t *current_prop;
+	unsigned long long gic_affinity_val;
+	unsigned int ctlr_enable = 0U;
+
+	/* Make sure there's a valid property array */
+	if (interrupt_props_num > 0U) {
+		assert(interrupt_props != NULL);
+	}
+
+	for (i = 0U; i < interrupt_props_num; i++) {
+		current_prop = &interrupt_props[i];
+
+		unsigned int intr_num = current_prop->intr_num;
+
+		/* Skip SGI, (E)PPI and LPI interrupts */
+		if (!IS_SPI(intr_num)) {
+			continue;
+		}
+
+		/* Configure this interrupt as a secure interrupt */
+		gicd_clr_igroupr(gicd_base, intr_num);
+
+		/* Configure this interrupt as G0 or a G1S interrupt */
+		assert((current_prop->intr_grp == INTR_GROUP0) ||
+				(current_prop->intr_grp == INTR_GROUP1S));
+
+		if (current_prop->intr_grp == INTR_GROUP1S) {
+			gicd_set_igrpmodr(gicd_base, intr_num);
+			ctlr_enable |= CTLR_ENABLE_G1S_BIT;
+		} else {
+			gicd_clr_igrpmodr(gicd_base, intr_num);
+			ctlr_enable |= CTLR_ENABLE_G0_BIT;
+		}
+
+		/* Set interrupt configuration */
+		gicd_set_icfgr(gicd_base, intr_num, current_prop->intr_cfg);
+
+		/* Set the priority of this interrupt */
+		gicd_set_ipriorityr(gicd_base, intr_num,
+					current_prop->intr_pri);
+
+		/* Target (E)SPIs to the primary CPU */
+		gic_affinity_val =
+			gicd_irouter_val_from_mpidr(read_mpidr(), 0U);
+		gicd_write_irouter(gicd_base, intr_num,
+					gic_affinity_val);
+
+		/* Enable this interrupt */
+		gicd_set_isenabler(gicd_base, intr_num);
+	}
+
+	return ctlr_enable;
+}
+
+/*******************************************************************************
+ * Helper function to configure the default attributes of (E)SPIs
+ ******************************************************************************/
+void gicv3_ppi_sgi_config_defaults(uintptr_t gicr_base)
+{
+	unsigned int i, ppi_regs_num, regs_num;
+
+#if GIC_EXT_INTID
+	/* Calculate number of PPI registers */
+	ppi_regs_num = (unsigned int)((gicr_read_typer(gicr_base) >>
+			TYPER_PPI_NUM_SHIFT) & TYPER_PPI_NUM_MASK) + 1;
+	/* All other values except PPInum [0-2] are reserved */
+	if (ppi_regs_num > 3U) {
+		ppi_regs_num = 1U;
+	}
+#else
+	ppi_regs_num = 1U;
+#endif
+	/*
+	 * Disable all SGIs (imp. def.)/(E)PPIs before configuring them.
+	 * This is a more scalable approach as it avoids clearing
+	 * the enable bits in the GICD_CTLR.
+	 */
+	for (i = 0U; i < ppi_regs_num; ++i) {
+		gicr_write_icenabler(gicr_base, i, ~0U);
+	}
+
+	/* Wait for pending writes to GICR_ICENABLER */
+	gicr_wait_for_pending_write(gicr_base);
+
+	/* 32 interrupt IDs per GICR_IGROUPR register */
+	for (i = 0U; i < ppi_regs_num; ++i) {
+		/* Treat all SGIs/(E)PPIs as G1NS by default */
+		gicr_write_igroupr(gicr_base, i, ~0U);
+	}
+
+	/* 4 interrupt IDs per GICR_IPRIORITYR register */
+	regs_num = ppi_regs_num << 3;
+	for (i = 0U; i < regs_num; ++i) {
+		/* Setup the default (E)PPI/SGI priorities doing 4 at a time */
+		gicr_write_ipriorityr(gicr_base, i, GICD_IPRIORITYR_DEF_VAL);
+	}
+
+	/* 16 interrupt IDs per GICR_ICFGR register */
+	regs_num = ppi_regs_num << 1;
+	for (i = (MIN_PPI_ID >> ICFGR_SHIFT); i < regs_num; ++i) {
+		/* Configure all (E)PPIs as level triggered by default */
+		gicr_write_icfgr(gicr_base, i, 0U);
+	}
+}
+
+/*******************************************************************************
+ * Helper function to configure properties of secure G0 and G1S (E)PPIs and SGIs
+ ******************************************************************************/
+unsigned int gicv3_secure_ppi_sgi_config_props(uintptr_t gicr_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num)
+{
+	unsigned int i;
+	const interrupt_prop_t *current_prop;
+	unsigned int ctlr_enable = 0U;
+
+	/* Make sure there's a valid property array */
+	if (interrupt_props_num > 0U) {
+		assert(interrupt_props != NULL);
+	}
+
+	for (i = 0U; i < interrupt_props_num; i++) {
+		current_prop = &interrupt_props[i];
+
+		unsigned int intr_num = current_prop->intr_num;
+
+		/* Skip (E)SPI interrupt */
+		if (!IS_SGI_PPI(intr_num)) {
+			continue;
+		}
+
+		/* Configure this interrupt as a secure interrupt */
+		gicr_clr_igroupr(gicr_base, intr_num);
+
+		/* Configure this interrupt as G0 or a G1S interrupt */
+		assert((current_prop->intr_grp == INTR_GROUP0) ||
+			(current_prop->intr_grp == INTR_GROUP1S));
+
+		if (current_prop->intr_grp == INTR_GROUP1S) {
+			gicr_set_igrpmodr(gicr_base, intr_num);
+			ctlr_enable |= CTLR_ENABLE_G1S_BIT;
+		} else {
+			gicr_clr_igrpmodr(gicr_base, intr_num);
+			ctlr_enable |= CTLR_ENABLE_G0_BIT;
+		}
+
+		/* Set the priority of this interrupt */
+		gicr_set_ipriorityr(gicr_base, intr_num,
+					current_prop->intr_pri);
+
+		/*
+		 * Set interrupt configuration for (E)PPIs.
+		 * Configurations for SGIs 0-15 are ignored.
+		 */
+		if (intr_num >= MIN_PPI_ID) {
+			gicr_set_icfgr(gicr_base, intr_num,
+					current_prop->intr_cfg);
+		}
+
+		/* Enable this interrupt */
+		gicr_set_isenabler(gicr_base, intr_num);
+	}
+
+	return ctlr_enable;
+}
+
+/**
+ * gicv3_rdistif_get_number_frames() - determine size of GICv3 GICR region
+ * @gicr_frame: base address of the GICR region to check
+ *
+ * This iterates over the GICR_TYPER registers of multiple GICR frames in
+ * a GICR region, to find the instance which has the LAST bit set. For most
+ * systems this corresponds to the number of cores handled by a redistributor,
+ * but there could be disabled cores among them.
+ * It assumes that each GICR region is fully accessible (till the LAST bit
+ * marks the end of the region).
+ * If a platform has multiple GICR regions, this function would need to be
+ * called multiple times, providing the respective GICR base address each time.
+ *
+ * Return: number of valid GICR frames (at least 1, up to PLATFORM_CORE_COUNT)
+ ******************************************************************************/
+unsigned int gicv3_rdistif_get_number_frames(const uintptr_t gicr_frame)
+{
+	uintptr_t rdistif_base = gicr_frame;
+	unsigned int count;
+
+	for (count = 1U; count < PLATFORM_CORE_COUNT; count++) {
+		uint64_t typer_val = gicr_read_typer(rdistif_base);
+
+		if ((typer_val & TYPER_LAST_BIT) != 0U) {
+			break;
+		}
+		rdistif_base += gicv3_redist_size(typer_val);
+	}
+
+	return count;
+}
+
+unsigned int gicv3_get_component_partnum(const uintptr_t gic_frame)
+{
+	unsigned int part_id;
+
+	/*
+	 * The lower 8 bits of PIDR0, complemented by the lower 4 bits of
+	 * PIDR1 contain a part number identifying the GIC component at a
+	 * particular base address.
+	 */
+	part_id = mmio_read_32(gic_frame + GICD_PIDR0_GICV3) & 0xff;
+	part_id |= (mmio_read_32(gic_frame + GICD_PIDR1_GICV3) << 8) & 0xf00;
+
+	return part_id;
+}
+
+/*******************************************************************************
+ * Helper function to return product ID and revision of GIC
+ * @gicd_base:   base address of the GIC distributor
+ * @gic_prod_id: retrieved product id of GIC
+ * @gic_rev:     retrieved revision of GIC
+ ******************************************************************************/
+void gicv3_get_component_prodid_rev(const uintptr_t gicd_base,
+				    unsigned int *gic_prod_id,
+				    uint8_t *gic_rev)
+{
+	unsigned int gicd_iidr;
+	uint8_t gic_variant;
+
+	gicd_iidr = gicd_read_iidr(gicd_base);
+	*gic_prod_id = gicd_iidr >> IIDR_PRODUCT_ID_SHIFT;
+	*gic_prod_id &= IIDR_PRODUCT_ID_MASK;
+
+	gic_variant = gicd_iidr >> IIDR_VARIANT_SHIFT;
+	gic_variant &= IIDR_VARIANT_MASK;
+
+	*gic_rev = gicd_iidr >> IIDR_REV_SHIFT;
+	*gic_rev &= IIDR_REV_MASK;
+
+	/*
+	 * pack gic variant and gic_rev in 1 byte
+	 * gic_rev = gic_variant[7:4] and gic_rev[0:3]
+	 */
+	*gic_rev = *gic_rev | gic_variant << 0x4;
+
+}
diff --git a/drivers/arm/gic/v3/gicv3_main.c b/drivers/arm/gic/v3/gicv3_main.c
new file mode 100644
index 0000000..bc93f93
--- /dev/null
+++ b/drivers/arm/gic/v3/gicv3_main.c
@@ -0,0 +1,1361 @@
+/*
+ * Copyright (c) 2015-2022, Arm Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <common/debug.h>
+#include <common/interrupt_props.h>
+#include <drivers/arm/gicv3.h>
+#include <lib/spinlock.h>
+
+#include "gicv3_private.h"
+
+const gicv3_driver_data_t *gicv3_driver_data;
+
+/*
+ * Spinlock to guard registers needing read-modify-write. APIs protected by this
+ * spinlock are used either at boot time (when only a single CPU is active), or
+ * when the system is fully coherent.
+ */
+static spinlock_t gic_lock;
+
+/*
+ * Redistributor power operations are weakly bound so that they can be
+ * overridden
+ */
+#pragma weak gicv3_rdistif_off
+#pragma weak gicv3_rdistif_on
+
+/* Check interrupt ID for SGI/(E)PPI and (E)SPIs */
+static bool is_sgi_ppi(unsigned int id);
+
+/*
+ * Helper macros to save and restore GICR and GICD registers
+ * corresponding to their numbers to and from the context
+ */
+#define RESTORE_GICR_REG(base, ctx, name, i)	\
+	gicr_write_##name((base), (i), (ctx)->gicr_##name[(i)])
+
+#define SAVE_GICR_REG(base, ctx, name, i)	\
+	(ctx)->gicr_##name[(i)] = gicr_read_##name((base), (i))
+
+/* Helper macros to save and restore GICD registers to and from the context */
+#define RESTORE_GICD_REGS(base, ctx, intr_num, reg, REG)		\
+	do {								\
+		for (unsigned int int_id = MIN_SPI_ID; int_id < (intr_num);\
+				int_id += (1U << REG##R_SHIFT)) {	\
+			gicd_write_##reg((base), int_id,		\
+				(ctx)->gicd_##reg[(int_id - MIN_SPI_ID) >> \
+							REG##R_SHIFT]);	\
+		}							\
+	} while (false)
+
+#define SAVE_GICD_REGS(base, ctx, intr_num, reg, REG)			\
+	do {								\
+		for (unsigned int int_id = MIN_SPI_ID; int_id < (intr_num);\
+				int_id += (1U << REG##R_SHIFT)) {	\
+			(ctx)->gicd_##reg[(int_id - MIN_SPI_ID) >>	\
+			REG##R_SHIFT] = gicd_read_##reg((base), int_id); \
+		}							\
+	} while (false)
+
+#if GIC_EXT_INTID
+#define RESTORE_GICD_EREGS(base, ctx, intr_num, reg, REG)		\
+	do {								\
+		for (unsigned int int_id = MIN_ESPI_ID; int_id < (intr_num);\
+				int_id += (1U << REG##R_SHIFT)) {	\
+			gicd_write_##reg((base), int_id,		\
+			(ctx)->gicd_##reg[(int_id - (MIN_ESPI_ID -	\
+			round_up(TOTAL_SPI_INTR_NUM, 1U << REG##R_SHIFT)))\
+						>> REG##R_SHIFT]);	\
+		}							\
+	} while (false)
+
+#define SAVE_GICD_EREGS(base, ctx, intr_num, reg, REG)			\
+	do {								\
+		for (unsigned int int_id = MIN_ESPI_ID; int_id < (intr_num);\
+				int_id += (1U << REG##R_SHIFT)) {	\
+			(ctx)->gicd_##reg[(int_id - (MIN_ESPI_ID -	\
+			round_up(TOTAL_SPI_INTR_NUM, 1U << REG##R_SHIFT)))\
+			>> REG##R_SHIFT] = gicd_read_##reg((base), int_id);\
+		}							\
+	} while (false)
+#else
+#define SAVE_GICD_EREGS(base, ctx, intr_num, reg, REG)
+#define RESTORE_GICD_EREGS(base, ctx, intr_num, reg, REG)
+#endif /* GIC_EXT_INTID */
+
+/*******************************************************************************
+ * This function initialises the ARM GICv3 driver in EL3 with provided platform
+ * inputs.
+ ******************************************************************************/
+void __init gicv3_driver_init(const gicv3_driver_data_t *plat_driver_data)
+{
+	unsigned int gic_version;
+	unsigned int gicv2_compat;
+
+	assert(plat_driver_data != NULL);
+	assert(plat_driver_data->gicd_base != 0U);
+	assert(plat_driver_data->rdistif_num != 0U);
+	assert(plat_driver_data->rdistif_base_addrs != NULL);
+
+	assert(IS_IN_EL3());
+
+	assert((plat_driver_data->interrupt_props_num != 0U) ?
+	       (plat_driver_data->interrupt_props != NULL) : 1);
+
+	/* Check for system register support */
+#ifndef __aarch64__
+	assert((read_id_pfr1() &
+			(ID_PFR1_GIC_MASK << ID_PFR1_GIC_SHIFT)) != 0U);
+#else
+	assert((read_id_aa64pfr0_el1() &
+			(ID_AA64PFR0_GIC_MASK << ID_AA64PFR0_GIC_SHIFT)) != 0U);
+#endif /* !__aarch64__ */
+
+	gic_version = gicd_read_pidr2(plat_driver_data->gicd_base);
+	gic_version >>= PIDR2_ARCH_REV_SHIFT;
+	gic_version &= PIDR2_ARCH_REV_MASK;
+
+	/* Check GIC version */
+#if !GIC_ENABLE_V4_EXTN
+	assert(gic_version == ARCH_REV_GICV3);
+#endif
+	/*
+	 * Find out whether the GIC supports the GICv2 compatibility mode.
+	 * The ARE_S bit resets to 0 if supported
+	 */
+	gicv2_compat = gicd_read_ctlr(plat_driver_data->gicd_base);
+	gicv2_compat >>= CTLR_ARE_S_SHIFT;
+	gicv2_compat = gicv2_compat & CTLR_ARE_S_MASK;
+
+	if (plat_driver_data->gicr_base != 0U) {
+		/*
+		 * Find the base address of each implemented Redistributor interface.
+		 * The number of interfaces should be equal to the number of CPUs in the
+		 * system. The memory for saving these addresses has to be allocated by
+		 * the platform port
+		 */
+		gicv3_rdistif_base_addrs_probe(plat_driver_data->rdistif_base_addrs,
+						   plat_driver_data->rdistif_num,
+						   plat_driver_data->gicr_base,
+						   plat_driver_data->mpidr_to_core_pos);
+#if !HW_ASSISTED_COHERENCY
+		/*
+		 * Flush the rdistif_base_addrs[] contents linked to the GICv3 driver.
+		 */
+		flush_dcache_range((uintptr_t)(plat_driver_data->rdistif_base_addrs),
+			plat_driver_data->rdistif_num *
+			sizeof(*(plat_driver_data->rdistif_base_addrs)));
+#endif
+	}
+	gicv3_driver_data = plat_driver_data;
+
+	/*
+	 * The GIC driver data is initialized by the primary CPU with caches
+	 * enabled. When the secondary CPU boots up, it initializes the
+	 * GICC/GICR interface with the caches disabled. Hence flush the
+	 * driver data to ensure coherency. This is not required if the
+	 * platform has HW_ASSISTED_COHERENCY enabled.
+	 */
+#if !HW_ASSISTED_COHERENCY
+	flush_dcache_range((uintptr_t)&gicv3_driver_data,
+		sizeof(gicv3_driver_data));
+	flush_dcache_range((uintptr_t)gicv3_driver_data,
+		sizeof(*gicv3_driver_data));
+#endif
+	gicv3_check_erratas_applies(plat_driver_data->gicd_base);
+
+	INFO("GICv%u with%s legacy support detected.\n", gic_version,
+				(gicv2_compat == 0U) ? "" : "out");
+	INFO("ARM GICv%u driver initialized in EL3\n", gic_version);
+}
+
+/*******************************************************************************
+ * This function initialises the GIC distributor interface based upon the data
+ * provided by the platform while initialising the driver.
+ ******************************************************************************/
+void __init gicv3_distif_init(void)
+{
+	unsigned int bitmap;
+
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+
+	assert(IS_IN_EL3());
+
+	/*
+	 * Clear the "enable" bits for G0/G1S/G1NS interrupts before configuring
+	 * the ARE_S bit. The Distributor might generate a system error
+	 * otherwise.
+	 */
+	gicd_clr_ctlr(gicv3_driver_data->gicd_base,
+		      CTLR_ENABLE_G0_BIT |
+		      CTLR_ENABLE_G1S_BIT |
+		      CTLR_ENABLE_G1NS_BIT,
+		      RWP_TRUE);
+
+	/* Set the ARE_S and ARE_NS bit now that interrupts have been disabled */
+	gicd_set_ctlr(gicv3_driver_data->gicd_base,
+			CTLR_ARE_S_BIT | CTLR_ARE_NS_BIT, RWP_TRUE);
+
+	/* Set the default attribute of all (E)SPIs */
+	gicv3_spis_config_defaults(gicv3_driver_data->gicd_base);
+
+	bitmap = gicv3_secure_spis_config_props(
+			gicv3_driver_data->gicd_base,
+			gicv3_driver_data->interrupt_props,
+			gicv3_driver_data->interrupt_props_num);
+
+	/* Enable the secure (E)SPIs now that they have been configured */
+	gicd_set_ctlr(gicv3_driver_data->gicd_base, bitmap, RWP_TRUE);
+}
+
+/*******************************************************************************
+ * This function initialises the GIC Redistributor interface of the calling CPU
+ * (identified by the 'proc_num' parameter) based upon the data provided by the
+ * platform while initialising the driver.
+ ******************************************************************************/
+void gicv3_rdistif_init(unsigned int proc_num)
+{
+	uintptr_t gicr_base;
+	unsigned int bitmap;
+	uint32_t ctlr;
+
+	assert(gicv3_driver_data != NULL);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+
+	ctlr = gicd_read_ctlr(gicv3_driver_data->gicd_base);
+	assert((ctlr & CTLR_ARE_S_BIT) != 0U);
+
+	assert(IS_IN_EL3());
+
+	/* Power on redistributor */
+	gicv3_rdistif_on(proc_num);
+
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+	assert(gicr_base != 0U);
+
+	/* Set the default attribute of all SGIs and (E)PPIs */
+	gicv3_ppi_sgi_config_defaults(gicr_base);
+
+	bitmap = gicv3_secure_ppi_sgi_config_props(gicr_base,
+			gicv3_driver_data->interrupt_props,
+			gicv3_driver_data->interrupt_props_num);
+
+	/* Enable interrupt groups as required, if not already */
+	if ((ctlr & bitmap) != bitmap) {
+		gicd_set_ctlr(gicv3_driver_data->gicd_base, bitmap, RWP_TRUE);
+	}
+}
+
+/*******************************************************************************
+ * Functions to perform power operations on GIC Redistributor
+ ******************************************************************************/
+void gicv3_rdistif_off(unsigned int proc_num)
+{
+}
+
+void gicv3_rdistif_on(unsigned int proc_num)
+{
+}
+
+/*******************************************************************************
+ * This function enables the GIC CPU interface of the calling CPU using only
+ * system register accesses.
+ ******************************************************************************/
+void gicv3_cpuif_enable(unsigned int proc_num)
+{
+	uintptr_t gicr_base;
+	u_register_t scr_el3;
+	unsigned int icc_sre_el3;
+
+	assert(gicv3_driver_data != NULL);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+	assert(IS_IN_EL3());
+
+	/* Mark the connected core as awake */
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+	gicv3_rdistif_mark_core_awake(gicr_base);
+
+	/* Disable the legacy interrupt bypass */
+	icc_sre_el3 = ICC_SRE_DIB_BIT | ICC_SRE_DFB_BIT;
+
+	/*
+	 * Enable system register access for EL3 and allow lower exception
+	 * levels to configure the same for themselves. If the legacy mode is
+	 * not supported, the SRE bit is RAO/WI
+	 */
+	icc_sre_el3 |= (ICC_SRE_EN_BIT | ICC_SRE_SRE_BIT);
+	write_icc_sre_el3(read_icc_sre_el3() | icc_sre_el3);
+
+	scr_el3 = read_scr_el3();
+
+	/*
+	 * Switch to NS state to write Non secure ICC_SRE_EL1 and
+	 * ICC_SRE_EL2 registers.
+	 */
+	write_scr_el3(scr_el3 | SCR_NS_BIT);
+	isb();
+
+	write_icc_sre_el2(read_icc_sre_el2() | icc_sre_el3);
+	write_icc_sre_el1(ICC_SRE_SRE_BIT);
+	isb();
+
+	/* Switch to secure state. */
+	write_scr_el3(scr_el3 & (~SCR_NS_BIT));
+	isb();
+
+	/* Write the secure ICC_SRE_EL1 register */
+	write_icc_sre_el1(ICC_SRE_SRE_BIT);
+	isb();
+
+	/* Program the idle priority in the PMR */
+	write_icc_pmr_el1(GIC_PRI_MASK);
+
+	/* Enable Group0 interrupts */
+	write_icc_igrpen0_el1(IGRPEN1_EL1_ENABLE_G0_BIT);
+
+	/* Enable Group1 Secure interrupts */
+	write_icc_igrpen1_el3(read_icc_igrpen1_el3() |
+				IGRPEN1_EL3_ENABLE_G1S_BIT);
+	isb();
+	/* Add DSB to ensure visibility of System register writes */
+	dsb();
+}
+
+/*******************************************************************************
+ * This function disables the GIC CPU interface of the calling CPU using
+ * only system register accesses.
+ ******************************************************************************/
+void gicv3_cpuif_disable(unsigned int proc_num)
+{
+	uintptr_t gicr_base;
+
+	assert(gicv3_driver_data != NULL);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	assert(IS_IN_EL3());
+
+	/* Disable legacy interrupt bypass */
+	write_icc_sre_el3(read_icc_sre_el3() |
+			  (ICC_SRE_DIB_BIT | ICC_SRE_DFB_BIT));
+
+	/* Disable Group0 interrupts */
+	write_icc_igrpen0_el1(read_icc_igrpen0_el1() &
+			      ~IGRPEN1_EL1_ENABLE_G0_BIT);
+
+	/* Disable Group1 Secure and Non-Secure interrupts */
+	write_icc_igrpen1_el3(read_icc_igrpen1_el3() &
+			      ~(IGRPEN1_EL3_ENABLE_G1NS_BIT |
+			      IGRPEN1_EL3_ENABLE_G1S_BIT));
+
+	/* Synchronise accesses to group enable registers */
+	isb();
+	/* Add DSB to ensure visibility of System register writes */
+	dsb();
+
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+	assert(gicr_base != 0UL);
+
+	/*
+	 * dsb() already issued previously after clearing the CPU group
+	 * enabled, apply below workaround to toggle the "DPG*"
+	 * bits of GICR_CTLR register for unblocking event.
+	 */
+	gicv3_apply_errata_wa_2384374(gicr_base);
+
+	/* Mark the connected core as asleep */
+	gicv3_rdistif_mark_core_asleep(gicr_base);
+}
+
+/*******************************************************************************
+ * This function returns the id of the highest priority pending interrupt at
+ * the GIC cpu interface.
+ ******************************************************************************/
+unsigned int gicv3_get_pending_interrupt_id(void)
+{
+	unsigned int id;
+
+	assert(IS_IN_EL3());
+	id = (uint32_t)read_icc_hppir0_el1() & HPPIR0_EL1_INTID_MASK;
+
+	/*
+	 * If the ID is special identifier corresponding to G1S or G1NS
+	 * interrupt, then read the highest pending group 1 interrupt.
+	 */
+	if ((id == PENDING_G1S_INTID) || (id == PENDING_G1NS_INTID)) {
+		return (uint32_t)read_icc_hppir1_el1() & HPPIR1_EL1_INTID_MASK;
+	}
+
+	return id;
+}
+
+/*******************************************************************************
+ * This function returns the type of the highest priority pending interrupt at
+ * the GIC cpu interface. The return values can be one of the following :
+ *   PENDING_G1S_INTID  : The interrupt type is secure Group 1.
+ *   PENDING_G1NS_INTID : The interrupt type is non secure Group 1.
+ *   0 - 1019           : The interrupt type is secure Group 0.
+ *   GIC_SPURIOUS_INTERRUPT : there is no pending interrupt with
+ *                            sufficient priority to be signaled
+ ******************************************************************************/
+unsigned int gicv3_get_pending_interrupt_type(void)
+{
+	assert(IS_IN_EL3());
+	return (uint32_t)read_icc_hppir0_el1() & HPPIR0_EL1_INTID_MASK;
+}
+
+/*******************************************************************************
+ * This function returns the type of the interrupt id depending upon the group
+ * this interrupt has been configured under by the interrupt controller i.e.
+ * group0 or group1 Secure / Non Secure. The return value can be one of the
+ * following :
+ *    INTR_GROUP0  : The interrupt type is a Secure Group 0 interrupt
+ *    INTR_GROUP1S : The interrupt type is a Secure Group 1 secure interrupt
+ *    INTR_GROUP1NS: The interrupt type is a Secure Group 1 non secure
+ *                   interrupt.
+ ******************************************************************************/
+unsigned int gicv3_get_interrupt_type(unsigned int id, unsigned int proc_num)
+{
+	unsigned int igroup, grpmodr;
+	uintptr_t gicr_base;
+
+	assert(IS_IN_EL3());
+	assert(gicv3_driver_data != NULL);
+
+	/* Ensure the parameters are valid */
+	assert((id < PENDING_G1S_INTID) || (id >= MIN_LPI_ID));
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+
+	/* All LPI interrupts are Group 1 non secure */
+	if (id >= MIN_LPI_ID) {
+		return INTR_GROUP1NS;
+	}
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* SGIs: 0-15, PPIs: 16-31, EPPIs: 1056-1119 */
+		assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+		gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+		igroup = gicr_get_igroupr(gicr_base, id);
+		grpmodr = gicr_get_igrpmodr(gicr_base, id);
+	} else {
+		/* SPIs: 32-1019, ESPIs: 4096-5119 */
+		assert(gicv3_driver_data->gicd_base != 0U);
+		igroup = gicd_get_igroupr(gicv3_driver_data->gicd_base, id);
+		grpmodr = gicd_get_igrpmodr(gicv3_driver_data->gicd_base, id);
+	}
+
+	/*
+	 * If the IGROUP bit is set, then it is a Group 1 Non secure
+	 * interrupt
+	 */
+	if (igroup != 0U) {
+		return INTR_GROUP1NS;
+	}
+
+	/* If the GRPMOD bit is set, then it is a Group 1 Secure interrupt */
+	if (grpmodr != 0U) {
+		return INTR_GROUP1S;
+	}
+
+	/* Else it is a Group 0 Secure interrupt */
+	return INTR_GROUP0;
+}
+
+/*****************************************************************************
+ * Function to save and disable the GIC ITS register context. The power
+ * management of GIC ITS is implementation-defined and this function doesn't
+ * save any memory structures required to support ITS. As the sequence to save
+ * this state is implementation defined, it should be executed in platform
+ * specific code. Calling this function alone and then powering down the GIC and
+ * ITS without implementing the aforementioned platform specific code will
+ * corrupt the ITS state.
+ *
+ * This function must be invoked after the GIC CPU interface is disabled.
+ *****************************************************************************/
+void gicv3_its_save_disable(uintptr_t gits_base,
+				gicv3_its_ctx_t * const its_ctx)
+{
+	unsigned int i;
+
+	assert(gicv3_driver_data != NULL);
+	assert(IS_IN_EL3());
+	assert(its_ctx != NULL);
+	assert(gits_base != 0U);
+
+	its_ctx->gits_ctlr = gits_read_ctlr(gits_base);
+
+	/* Disable the ITS */
+	gits_write_ctlr(gits_base, its_ctx->gits_ctlr & ~GITS_CTLR_ENABLED_BIT);
+
+	/* Wait for quiescent state */
+	gits_wait_for_quiescent_bit(gits_base);
+
+	its_ctx->gits_cbaser = gits_read_cbaser(gits_base);
+	its_ctx->gits_cwriter = gits_read_cwriter(gits_base);
+
+	for (i = 0U; i < ARRAY_SIZE(its_ctx->gits_baser); i++) {
+		its_ctx->gits_baser[i] = gits_read_baser(gits_base, i);
+	}
+}
+
+/*****************************************************************************
+ * Function to restore the GIC ITS register context. The power
+ * management of GIC ITS is implementation defined and this function doesn't
+ * restore any memory structures required to support ITS. The assumption is
+ * that these structures are in memory and are retained during system suspend.
+ *
+ * This must be invoked before the GIC CPU interface is enabled.
+ *****************************************************************************/
+void gicv3_its_restore(uintptr_t gits_base,
+			const gicv3_its_ctx_t * const its_ctx)
+{
+	unsigned int i;
+
+	assert(gicv3_driver_data != NULL);
+	assert(IS_IN_EL3());
+	assert(its_ctx != NULL);
+	assert(gits_base != 0U);
+
+	/* Assert that the GITS is disabled and quiescent */
+	assert((gits_read_ctlr(gits_base) & GITS_CTLR_ENABLED_BIT) == 0U);
+	assert((gits_read_ctlr(gits_base) & GITS_CTLR_QUIESCENT_BIT) != 0U);
+
+	gits_write_cbaser(gits_base, its_ctx->gits_cbaser);
+	gits_write_cwriter(gits_base, its_ctx->gits_cwriter);
+
+	for (i = 0U; i < ARRAY_SIZE(its_ctx->gits_baser); i++) {
+		gits_write_baser(gits_base, i, its_ctx->gits_baser[i]);
+	}
+
+	/* Restore the ITS CTLR but leave the ITS disabled */
+	gits_write_ctlr(gits_base, its_ctx->gits_ctlr & ~GITS_CTLR_ENABLED_BIT);
+}
+
+/*****************************************************************************
+ * Function to save the GIC Redistributor register context. This function
+ * must be invoked after CPU interface disable and prior to Distributor save.
+ *****************************************************************************/
+void gicv3_rdistif_save(unsigned int proc_num,
+			gicv3_redist_ctx_t * const rdist_ctx)
+{
+	uintptr_t gicr_base;
+	unsigned int i, ppi_regs_num, regs_num;
+
+	assert(gicv3_driver_data != NULL);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+	assert(IS_IN_EL3());
+	assert(rdist_ctx != NULL);
+
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+
+#if GIC_EXT_INTID
+	/* Calculate number of PPI registers */
+	ppi_regs_num = (unsigned int)((gicr_read_typer(gicr_base) >>
+			TYPER_PPI_NUM_SHIFT) & TYPER_PPI_NUM_MASK) + 1;
+	/* All other values except PPInum [0-2] are reserved */
+	if (ppi_regs_num > 3U) {
+		ppi_regs_num = 1U;
+	}
+#else
+	ppi_regs_num = 1U;
+#endif
+	/*
+	 * Wait for any write to GICR_CTLR to complete before trying to save any
+	 * state.
+	 */
+	gicr_wait_for_pending_write(gicr_base);
+
+	rdist_ctx->gicr_ctlr = gicr_read_ctlr(gicr_base);
+
+	rdist_ctx->gicr_propbaser = gicr_read_propbaser(gicr_base);
+	rdist_ctx->gicr_pendbaser = gicr_read_pendbaser(gicr_base);
+
+	/* 32 interrupt IDs per register */
+	for (i = 0U; i < ppi_regs_num; ++i) {
+		SAVE_GICR_REG(gicr_base, rdist_ctx, igroupr, i);
+		SAVE_GICR_REG(gicr_base, rdist_ctx, isenabler, i);
+		SAVE_GICR_REG(gicr_base, rdist_ctx, ispendr, i);
+		SAVE_GICR_REG(gicr_base, rdist_ctx, isactiver, i);
+		SAVE_GICR_REG(gicr_base, rdist_ctx, igrpmodr, i);
+	}
+
+	/* 16 interrupt IDs per GICR_ICFGR register */
+	regs_num = ppi_regs_num << 1;
+	for (i = 0U; i < regs_num; ++i) {
+		SAVE_GICR_REG(gicr_base, rdist_ctx, icfgr, i);
+	}
+
+	rdist_ctx->gicr_nsacr = gicr_read_nsacr(gicr_base);
+
+	/* 4 interrupt IDs per GICR_IPRIORITYR register */
+	regs_num = ppi_regs_num << 3;
+	for (i = 0U; i < regs_num; ++i) {
+		rdist_ctx->gicr_ipriorityr[i] =
+		gicr_ipriorityr_read(gicr_base, i);
+	}
+
+	/*
+	 * Call the pre-save hook that implements the IMP DEF sequence that may
+	 * be required on some GIC implementations. As this may need to access
+	 * the Redistributor registers, we pass it proc_num.
+	 */
+	gicv3_distif_pre_save(proc_num);
+}
+
+/*****************************************************************************
+ * Function to restore the GIC Redistributor register context. We disable
+ * LPI and per-cpu interrupts before we start restore of the Redistributor.
+ * This function must be invoked after Distributor restore but prior to
+ * CPU interface enable. The pending and active interrupts are restored
+ * after the interrupts are fully configured and enabled.
+ *****************************************************************************/
+void gicv3_rdistif_init_restore(unsigned int proc_num,
+				const gicv3_redist_ctx_t * const rdist_ctx)
+{
+	uintptr_t gicr_base;
+	unsigned int i, ppi_regs_num, regs_num;
+
+	assert(gicv3_driver_data != NULL);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+	assert(IS_IN_EL3());
+	assert(rdist_ctx != NULL);
+
+	gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+
+#if GIC_EXT_INTID
+	/* Calculate number of PPI registers */
+	ppi_regs_num = (unsigned int)((gicr_read_typer(gicr_base) >>
+			TYPER_PPI_NUM_SHIFT) & TYPER_PPI_NUM_MASK) + 1;
+	/* All other values except PPInum [0-2] are reserved */
+	if (ppi_regs_num > 3U) {
+		ppi_regs_num = 1U;
+	}
+#else
+	ppi_regs_num = 1U;
+#endif
+	/* Power on redistributor */
+	gicv3_rdistif_on(proc_num);
+
+	/*
+	 * Call the post-restore hook that implements the IMP DEF sequence that
+	 * may be required on some GIC implementations. As this may need to
+	 * access the Redistributor registers, we pass it proc_num.
+	 */
+	gicv3_distif_post_restore(proc_num);
+
+	/*
+	 * Disable all SGIs (imp. def.)/(E)PPIs before configuring them.
+	 * This is a more scalable approach as it avoids clearing the enable
+	 * bits in the GICD_CTLR.
+	 */
+	for (i = 0U; i < ppi_regs_num; ++i) {
+		gicr_write_icenabler(gicr_base, i, ~0U);
+	}
+
+	/* Wait for pending writes to GICR_ICENABLER */
+	gicr_wait_for_pending_write(gicr_base);
+
+	/*
+	 * Disable the LPIs to avoid unpredictable behavior when writing to
+	 * GICR_PROPBASER and GICR_PENDBASER.
+	 */
+	gicr_write_ctlr(gicr_base,
+			rdist_ctx->gicr_ctlr & ~(GICR_CTLR_EN_LPIS_BIT));
+
+	/* Restore registers' content */
+	gicr_write_propbaser(gicr_base, rdist_ctx->gicr_propbaser);
+	gicr_write_pendbaser(gicr_base, rdist_ctx->gicr_pendbaser);
+
+	/* 32 interrupt IDs per register */
+	for (i = 0U; i < ppi_regs_num; ++i) {
+		RESTORE_GICR_REG(gicr_base, rdist_ctx, igroupr, i);
+		RESTORE_GICR_REG(gicr_base, rdist_ctx, igrpmodr, i);
+	}
+
+	/* 4 interrupt IDs per GICR_IPRIORITYR register */
+	regs_num = ppi_regs_num << 3;
+	for (i = 0U; i < regs_num; ++i) {
+		gicr_ipriorityr_write(gicr_base, i,
+					rdist_ctx->gicr_ipriorityr[i]);
+	}
+
+	/* 16 interrupt IDs per GICR_ICFGR register */
+	regs_num = ppi_regs_num << 1;
+	for (i = 0U; i < regs_num; ++i) {
+		RESTORE_GICR_REG(gicr_base, rdist_ctx, icfgr, i);
+	}
+
+	gicr_write_nsacr(gicr_base, rdist_ctx->gicr_nsacr);
+
+	/* Restore after group and priorities are set.
+	 * 32 interrupt IDs per register
+	 */
+	for (i = 0U; i < ppi_regs_num; ++i) {
+		RESTORE_GICR_REG(gicr_base, rdist_ctx, ispendr, i);
+		RESTORE_GICR_REG(gicr_base, rdist_ctx, isactiver, i);
+	}
+
+	/*
+	 * Wait for all writes to the Distributor to complete before enabling
+	 * the SGI and (E)PPIs.
+	 */
+	gicr_wait_for_upstream_pending_write(gicr_base);
+
+	/* 32 interrupt IDs per GICR_ISENABLER register */
+	for (i = 0U; i < ppi_regs_num; ++i) {
+		RESTORE_GICR_REG(gicr_base, rdist_ctx, isenabler, i);
+	}
+
+	/*
+	 * Restore GICR_CTLR.Enable_LPIs bit and wait for pending writes in case
+	 * the first write to GICR_CTLR was still in flight (this write only
+	 * restores GICR_CTLR.Enable_LPIs and no waiting is required for this
+	 * bit).
+	 */
+	gicr_write_ctlr(gicr_base, rdist_ctx->gicr_ctlr);
+	gicr_wait_for_pending_write(gicr_base);
+}
+
+/*****************************************************************************
+ * Function to save the GIC Distributor register context. This function
+ * must be invoked after CPU interface disable and Redistributor save.
+ *****************************************************************************/
+void gicv3_distif_save(gicv3_dist_ctx_t * const dist_ctx)
+{
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(IS_IN_EL3());
+	assert(dist_ctx != NULL);
+
+	uintptr_t gicd_base = gicv3_driver_data->gicd_base;
+	unsigned int num_ints = gicv3_get_spi_limit(gicd_base);
+#if GIC_EXT_INTID
+	unsigned int num_eints = gicv3_get_espi_limit(gicd_base);
+#endif
+
+	/* Wait for pending write to complete */
+	gicd_wait_for_pending_write(gicd_base);
+
+	/* Save the GICD_CTLR */
+	dist_ctx->gicd_ctlr = gicd_read_ctlr(gicd_base);
+
+	/* Save GICD_IGROUPR for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, igroupr, IGROUP);
+
+	/* Save GICD_IGROUPRE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, igroupr, IGROUP);
+
+	/* Save GICD_ISENABLER for INT_IDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, isenabler, ISENABLE);
+
+	/* Save GICD_ISENABLERE for INT_IDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, isenabler, ISENABLE);
+
+	/* Save GICD_ISPENDR for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, ispendr, ISPEND);
+
+	/* Save GICD_ISPENDRE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints,	ispendr, ISPEND);
+
+	/* Save GICD_ISACTIVER for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, isactiver, ISACTIVE);
+
+	/* Save GICD_ISACTIVERE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, isactiver, ISACTIVE);
+
+	/* Save GICD_IPRIORITYR for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, ipriorityr, IPRIORITY);
+
+	/* Save GICD_IPRIORITYRE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, ipriorityr, IPRIORITY);
+
+	/* Save GICD_ICFGR for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, icfgr, ICFG);
+
+	/* Save GICD_ICFGRE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, icfgr, ICFG);
+
+	/* Save GICD_IGRPMODR for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, igrpmodr, IGRPMOD);
+
+	/* Save GICD_IGRPMODRE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, igrpmodr, IGRPMOD);
+
+	/* Save GICD_NSACR for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, nsacr, NSAC);
+
+	/* Save GICD_NSACRE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, nsacr, NSAC);
+
+	/* Save GICD_IROUTER for INTIDs 32 - 1019 */
+	SAVE_GICD_REGS(gicd_base, dist_ctx, num_ints, irouter, IROUTE);
+
+	/* Save GICD_IROUTERE for INTIDs 4096 - 5119 */
+	SAVE_GICD_EREGS(gicd_base, dist_ctx, num_eints, irouter, IROUTE);
+
+	/*
+	 * GICD_ITARGETSR<n> and GICD_SPENDSGIR<n> are RAZ/WI when
+	 * GICD_CTLR.ARE_(S|NS) bits are set which is the case for our GICv3
+	 * driver.
+	 */
+}
+
+/*****************************************************************************
+ * Function to restore the GIC Distributor register context. We disable G0, G1S
+ * and G1NS interrupt groups before we start restore of the Distributor. This
+ * function must be invoked prior to Redistributor restore and CPU interface
+ * enable. The pending and active interrupts are restored after the interrupts
+ * are fully configured and enabled.
+ *****************************************************************************/
+void gicv3_distif_init_restore(const gicv3_dist_ctx_t * const dist_ctx)
+{
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(IS_IN_EL3());
+	assert(dist_ctx != NULL);
+
+	uintptr_t gicd_base = gicv3_driver_data->gicd_base;
+
+	/*
+	 * Clear the "enable" bits for G0/G1S/G1NS interrupts before configuring
+	 * the ARE_S bit. The Distributor might generate a system error
+	 * otherwise.
+	 */
+	gicd_clr_ctlr(gicd_base,
+		      CTLR_ENABLE_G0_BIT |
+		      CTLR_ENABLE_G1S_BIT |
+		      CTLR_ENABLE_G1NS_BIT,
+		      RWP_TRUE);
+
+	/* Set the ARE_S and ARE_NS bit now that interrupts have been disabled */
+	gicd_set_ctlr(gicd_base, CTLR_ARE_S_BIT | CTLR_ARE_NS_BIT, RWP_TRUE);
+
+	unsigned int num_ints = gicv3_get_spi_limit(gicd_base);
+#if GIC_EXT_INTID
+	unsigned int num_eints = gicv3_get_espi_limit(gicd_base);
+#endif
+	/* Restore GICD_IGROUPR for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, igroupr, IGROUP);
+
+	/* Restore GICD_IGROUPRE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, igroupr, IGROUP);
+
+	/* Restore GICD_IPRIORITYR for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, ipriorityr, IPRIORITY);
+
+	/* Restore GICD_IPRIORITYRE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, ipriorityr, IPRIORITY);
+
+	/* Restore GICD_ICFGR for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, icfgr, ICFG);
+
+	/* Restore GICD_ICFGRE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, icfgr, ICFG);
+
+	/* Restore GICD_IGRPMODR for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, igrpmodr, IGRPMOD);
+
+	/* Restore GICD_IGRPMODRE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, igrpmodr, IGRPMOD);
+
+	/* Restore GICD_NSACR for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, nsacr, NSAC);
+
+	/* Restore GICD_NSACRE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, nsacr, NSAC);
+
+	/* Restore GICD_IROUTER for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, irouter, IROUTE);
+
+	/* Restore GICD_IROUTERE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, irouter, IROUTE);
+
+	/*
+	 * Restore ISENABLER(E), ISPENDR(E) and ISACTIVER(E) after
+	 * the interrupts are configured.
+	 */
+
+	/* Restore GICD_ISENABLER for INT_IDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, isenabler, ISENABLE);
+
+	/* Restore GICD_ISENABLERE for INT_IDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, isenabler, ISENABLE);
+
+	/* Restore GICD_ISPENDR for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, ispendr, ISPEND);
+
+	/* Restore GICD_ISPENDRE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, ispendr, ISPEND);
+
+	/* Restore GICD_ISACTIVER for INTIDs 32 - 1019 */
+	RESTORE_GICD_REGS(gicd_base, dist_ctx, num_ints, isactiver, ISACTIVE);
+
+	/* Restore GICD_ISACTIVERE for INTIDs 4096 - 5119 */
+	RESTORE_GICD_EREGS(gicd_base, dist_ctx, num_eints, isactiver, ISACTIVE);
+
+	/* Restore the GICD_CTLR */
+	gicd_write_ctlr(gicd_base, dist_ctx->gicd_ctlr);
+	gicd_wait_for_pending_write(gicd_base);
+}
+
+/*******************************************************************************
+ * This function gets the priority of the interrupt the processor is currently
+ * servicing.
+ ******************************************************************************/
+unsigned int gicv3_get_running_priority(void)
+{
+	return (unsigned int)read_icc_rpr_el1();
+}
+
+/*******************************************************************************
+ * This function checks if the interrupt identified by id is active (whether the
+ * state is either active, or active and pending). The proc_num is used if the
+ * interrupt is SGI or (E)PPI and programs the corresponding Redistributor
+ * interface.
+ ******************************************************************************/
+unsigned int gicv3_get_interrupt_active(unsigned int id, unsigned int proc_num)
+{
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* For SGIs: 0-15, PPIs: 16-31 and EPPIs: 1056-1119 */
+		return gicr_get_isactiver(
+			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
+	}
+
+	/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
+	return gicd_get_isactiver(gicv3_driver_data->gicd_base, id);
+}
+
+/*******************************************************************************
+ * This function enables the interrupt identified by id. The proc_num
+ * is used if the interrupt is SGI or PPI, and programs the corresponding
+ * Redistributor interface.
+ ******************************************************************************/
+void gicv3_enable_interrupt(unsigned int id, unsigned int proc_num)
+{
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	/*
+	 * Ensure that any shared variable updates depending on out of band
+	 * interrupt trigger are observed before enabling interrupt.
+	 */
+	dsbishst();
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* For SGIs: 0-15, PPIs: 16-31 and EPPIs: 1056-1119 */
+		gicr_set_isenabler(
+			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
+	} else {
+		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
+		gicd_set_isenabler(gicv3_driver_data->gicd_base, id);
+	}
+}
+
+/*******************************************************************************
+ * This function disables the interrupt identified by id. The proc_num
+ * is used if the interrupt is SGI or PPI, and programs the corresponding
+ * Redistributor interface.
+ ******************************************************************************/
+void gicv3_disable_interrupt(unsigned int id, unsigned int proc_num)
+{
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	/*
+	 * Disable interrupt, and ensure that any shared variable updates
+	 * depending on out of band interrupt trigger are observed afterwards.
+	 */
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* For SGIs: 0-15, PPIs: 16-31 and EPPIs: 1056-1119 */
+		gicr_set_icenabler(
+			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
+
+		/* Write to clear enable requires waiting for pending writes */
+		gicr_wait_for_pending_write(
+			gicv3_driver_data->rdistif_base_addrs[proc_num]);
+	} else {
+		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
+		gicd_set_icenabler(gicv3_driver_data->gicd_base, id);
+
+		/* Write to clear enable requires waiting for pending writes */
+		gicd_wait_for_pending_write(gicv3_driver_data->gicd_base);
+	}
+
+	dsbishst();
+}
+
+/*******************************************************************************
+ * This function sets the interrupt priority as supplied for the given interrupt
+ * id.
+ ******************************************************************************/
+void gicv3_set_interrupt_priority(unsigned int id, unsigned int proc_num,
+		unsigned int priority)
+{
+	uintptr_t gicr_base;
+
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* For SGIs: 0-15, PPIs: 16-31 and EPPIs: 1056-1119 */
+		gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+		gicr_set_ipriorityr(gicr_base, id, priority);
+	} else {
+		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
+		gicd_set_ipriorityr(gicv3_driver_data->gicd_base, id, priority);
+	}
+}
+
+/*******************************************************************************
+ * This function assigns group for the interrupt identified by id. The proc_num
+ * is used if the interrupt is SGI or (E)PPI, and programs the corresponding
+ * Redistributor interface. The group can be any of GICV3_INTR_GROUP*
+ ******************************************************************************/
+void gicv3_set_interrupt_type(unsigned int id, unsigned int proc_num,
+		unsigned int type)
+{
+	bool igroup = false, grpmod = false;
+	uintptr_t gicr_base;
+
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	switch (type) {
+	case INTR_GROUP1S:
+		igroup = false;
+		grpmod = true;
+		break;
+	case INTR_GROUP0:
+		igroup = false;
+		grpmod = false;
+		break;
+	case INTR_GROUP1NS:
+		igroup = true;
+		grpmod = false;
+		break;
+	default:
+		assert(false);
+		break;
+	}
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* For SGIs: 0-15, PPIs: 16-31 and EPPIs: 1056-1119 */
+		gicr_base = gicv3_driver_data->rdistif_base_addrs[proc_num];
+
+		igroup ? gicr_set_igroupr(gicr_base, id) :
+			 gicr_clr_igroupr(gicr_base, id);
+		grpmod ? gicr_set_igrpmodr(gicr_base, id) :
+			 gicr_clr_igrpmodr(gicr_base, id);
+	} else {
+		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
+
+		/* Serialize read-modify-write to Distributor registers */
+		spin_lock(&gic_lock);
+
+		igroup ? gicd_set_igroupr(gicv3_driver_data->gicd_base, id) :
+			 gicd_clr_igroupr(gicv3_driver_data->gicd_base, id);
+		grpmod ? gicd_set_igrpmodr(gicv3_driver_data->gicd_base, id) :
+			 gicd_clr_igrpmodr(gicv3_driver_data->gicd_base, id);
+
+		spin_unlock(&gic_lock);
+	}
+}
+
+/*******************************************************************************
+ * This function raises the specified SGI of the specified group.
+ *
+ * The target parameter must be a valid MPIDR in the system.
+ ******************************************************************************/
+void gicv3_raise_sgi(unsigned int sgi_num, gicv3_irq_group_t group,
+		u_register_t target)
+{
+	unsigned int tgt, aff3, aff2, aff1, aff0;
+	uint64_t sgi_val;
+
+	/* Verify interrupt number is in the SGI range */
+	assert((sgi_num >= MIN_SGI_ID) && (sgi_num < MIN_PPI_ID));
+
+	/* Extract affinity fields from target */
+	aff0 = MPIDR_AFFLVL0_VAL(target);
+	aff1 = MPIDR_AFFLVL1_VAL(target);
+	aff2 = MPIDR_AFFLVL2_VAL(target);
+	aff3 = MPIDR_AFFLVL3_VAL(target);
+
+	/*
+	 * Make target list from affinity 0, and ensure GICv3 SGI can target
+	 * this PE.
+	 */
+	assert(aff0 < GICV3_MAX_SGI_TARGETS);
+	tgt = BIT_32(aff0);
+
+	/* Raise SGI to PE specified by its affinity */
+	sgi_val = GICV3_SGIR_VALUE(aff3, aff2, aff1, sgi_num, SGIR_IRM_TO_AFF,
+			tgt);
+
+	/*
+	 * Ensure that any shared variable updates depending on out of band
+	 * interrupt trigger are observed before raising SGI.
+	 */
+	dsbishst();
+
+	switch (group) {
+	case GICV3_G0:
+		write_icc_sgi0r_el1(sgi_val);
+		break;
+	case GICV3_G1NS:
+		write_icc_asgi1r(sgi_val);
+		break;
+	case GICV3_G1S:
+		write_icc_sgi1r(sgi_val);
+		break;
+	default:
+		assert(false);
+		break;
+	}
+
+	isb();
+}
+
+/*******************************************************************************
+ * This function sets the interrupt routing for the given (E)SPI interrupt id.
+ * The interrupt routing is specified in routing mode and mpidr.
+ *
+ * The routing mode can be either of:
+ *  - GICV3_IRM_ANY
+ *  - GICV3_IRM_PE
+ *
+ * The mpidr is the affinity of the PE to which the interrupt will be routed,
+ * and is ignored for routing mode GICV3_IRM_ANY.
+ ******************************************************************************/
+void gicv3_set_spi_routing(unsigned int id, unsigned int irm, u_register_t mpidr)
+{
+	unsigned long long aff;
+	uint64_t router;
+
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+
+	assert((irm == GICV3_IRM_ANY) || (irm == GICV3_IRM_PE));
+
+	assert(IS_SPI(id));
+
+	aff = gicd_irouter_val_from_mpidr(mpidr, irm);
+	gicd_write_irouter(gicv3_driver_data->gicd_base, id, aff);
+
+	/*
+	 * In implementations that do not require 1 of N distribution of SPIs,
+	 * IRM might be RAZ/WI. Read back and verify IRM bit.
+	 */
+	if (irm == GICV3_IRM_ANY) {
+		router = gicd_read_irouter(gicv3_driver_data->gicd_base, id);
+		if (((router >> IROUTER_IRM_SHIFT) & IROUTER_IRM_MASK) == 0U) {
+			ERROR("GICv3 implementation doesn't support routing ANY\n");
+			panic();
+		}
+	}
+}
+
+/*******************************************************************************
+ * This function clears the pending status of an interrupt identified by id.
+ * The proc_num is used if the interrupt is SGI or (E)PPI, and programs the
+ * corresponding Redistributor interface.
+ ******************************************************************************/
+void gicv3_clear_interrupt_pending(unsigned int id, unsigned int proc_num)
+{
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	/*
+	 * Clear pending interrupt, and ensure that any shared variable updates
+	 * depending on out of band interrupt trigger are observed afterwards.
+	 */
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* For SGIs: 0-15, PPIs: 16-31 and EPPIs: 1056-1119 */
+		gicr_set_icpendr(
+			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
+	} else {
+		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
+		gicd_set_icpendr(gicv3_driver_data->gicd_base, id);
+	}
+
+	dsbishst();
+}
+
+/*******************************************************************************
+ * This function sets the pending status of an interrupt identified by id.
+ * The proc_num is used if the interrupt is SGI or PPI and programs the
+ * corresponding Redistributor interface.
+ ******************************************************************************/
+void gicv3_set_interrupt_pending(unsigned int id, unsigned int proc_num)
+{
+	assert(gicv3_driver_data != NULL);
+	assert(gicv3_driver_data->gicd_base != 0U);
+	assert(proc_num < gicv3_driver_data->rdistif_num);
+	assert(gicv3_driver_data->rdistif_base_addrs != NULL);
+
+	/*
+	 * Ensure that any shared variable updates depending on out of band
+	 * interrupt trigger are observed before setting interrupt pending.
+	 */
+	dsbishst();
+
+	/* Check interrupt ID */
+	if (is_sgi_ppi(id)) {
+		/* For SGIs: 0-15, PPIs: 16-31 and EPPIs: 1056-1119 */
+		gicr_set_ispendr(
+			gicv3_driver_data->rdistif_base_addrs[proc_num], id);
+	} else {
+		/* For SPIs: 32-1019 and ESPIs: 4096-5119 */
+		gicd_set_ispendr(gicv3_driver_data->gicd_base, id);
+	}
+}
+
+/*******************************************************************************
+ * This function sets the PMR register with the supplied value. Returns the
+ * original PMR.
+ ******************************************************************************/
+unsigned int gicv3_set_pmr(unsigned int mask)
+{
+	unsigned int old_mask;
+
+	old_mask = (unsigned int)read_icc_pmr_el1();
+
+	/*
+	 * Order memory updates w.r.t. PMR write, and ensure they're visible
+	 * before potential out of band interrupt trigger because of PMR update.
+	 * PMR system register writes are self-synchronizing, so no ISB required
+	 * thereafter.
+	 */
+	dsbishst();
+	write_icc_pmr_el1(mask);
+
+	return old_mask;
+}
+
+/*******************************************************************************
+ * This function delegates the responsibility of discovering the corresponding
+ * Redistributor frames to each CPU itself. It is a modified version of
+ * gicv3_rdistif_base_addrs_probe() and is executed by each CPU in the platform
+ * unlike the previous way in which only the Primary CPU did the discovery of
+ * all the Redistributor frames for every CPU. It also handles the scenario in
+ * which the frames of various CPUs are not contiguous in physical memory.
+ ******************************************************************************/
+int gicv3_rdistif_probe(const uintptr_t gicr_frame)
+{
+	u_register_t mpidr, mpidr_self;
+	unsigned int proc_num;
+	uint64_t typer_val;
+	uintptr_t rdistif_base;
+	bool gicr_frame_found = false;
+
+	assert(gicv3_driver_data->gicr_base == 0U);
+
+	/* Ensure this function is called with Data Cache enabled */
+#ifndef __aarch64__
+	assert((read_sctlr() & SCTLR_C_BIT) != 0U);
+#else
+	assert((read_sctlr_el3() & SCTLR_C_BIT) != 0U);
+#endif /* !__aarch64__ */
+
+	mpidr_self = read_mpidr_el1() & MPIDR_AFFINITY_MASK;
+	rdistif_base = gicr_frame;
+	do {
+		typer_val = gicr_read_typer(rdistif_base);
+		mpidr = mpidr_from_gicr_typer(typer_val);
+		if (gicv3_driver_data->mpidr_to_core_pos != NULL) {
+			proc_num = gicv3_driver_data->mpidr_to_core_pos(mpidr);
+		} else {
+			proc_num = (unsigned int)(typer_val >>
+				TYPER_PROC_NUM_SHIFT) & TYPER_PROC_NUM_MASK;
+		}
+		if (mpidr == mpidr_self) {
+			/* The base address doesn't need to be initialized on
+			 * every warm boot.
+			 */
+			if (gicv3_driver_data->rdistif_base_addrs[proc_num]
+								!= 0U) {
+				return 0;
+			}
+			gicv3_driver_data->rdistif_base_addrs[proc_num] =
+			rdistif_base;
+			gicr_frame_found = true;
+			break;
+		}
+		rdistif_base += gicv3_redist_size(typer_val);
+	} while ((typer_val & TYPER_LAST_BIT) == 0U);
+
+	if (!gicr_frame_found) {
+		return -1;
+	}
+
+	/*
+	 * Flush the driver data to ensure coherency. This is
+	 * not required if platform has HW_ASSISTED_COHERENCY
+	 * enabled.
+	 */
+#if !HW_ASSISTED_COHERENCY
+	/*
+	 * Flush the rdistif_base_addrs[] contents linked to the GICv3 driver.
+	 */
+	flush_dcache_range((uintptr_t)&(gicv3_driver_data->rdistif_base_addrs[proc_num]),
+		sizeof(*(gicv3_driver_data->rdistif_base_addrs)));
+#endif
+	return 0; /* Found matching GICR frame */
+}
+
+/******************************************************************************
+ * This function checks the interrupt ID and returns true for SGIs and (E)PPIs
+ * and false for (E)SPIs IDs.
+ *****************************************************************************/
+static bool is_sgi_ppi(unsigned int id)
+{
+	/* SGIs: 0-15, PPIs: 16-31, EPPIs: 1056-1119 */
+	if (IS_SGI_PPI(id)) {
+		return true;
+	}
+
+	/* SPIs: 32-1019, ESPIs: 4096-5119 */
+	if (IS_SPI(id)) {
+		return false;
+	}
+
+	assert(false);
+	panic();
+}
diff --git a/drivers/arm/gic/v3/gicv3_private.h b/drivers/arm/gic/v3/gicv3_private.h
new file mode 100644
index 0000000..3af0500
--- /dev/null
+++ b/drivers/arm/gic/v3/gicv3_private.h
@@ -0,0 +1,707 @@
+/*
+ * Copyright (c) 2015-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef GICV3_PRIVATE_H
+#define GICV3_PRIVATE_H
+
+#include <assert.h>
+#include <stdint.h>
+
+#include <drivers/arm/gic_common.h>
+#include <drivers/arm/gicv3.h>
+#include <lib/mmio.h>
+
+#include "../common/gic_common_private.h"
+
+/*******************************************************************************
+ * GICv3 private macro definitions
+ ******************************************************************************/
+
+/* Constants to indicate the status of the RWP bit */
+#define RWP_TRUE		U(1)
+#define RWP_FALSE		U(0)
+
+/* Calculate GIC register bit number corresponding to its interrupt ID */
+#define	BIT_NUM(REG, id)	\
+	((id) & ((1U << REG##R_SHIFT) - 1U))
+
+/*
+ * Calculate 8, 32 and 64-bit GICD register offset
+ * corresponding to its interrupt ID
+ */
+#if GIC_EXT_INTID
+	/* GICv3.1 */
+#define	GICD_OFFSET_8(REG, id)				\
+	(((id) <= MAX_SPI_ID) ?				\
+	GICD_##REG##R + (uintptr_t)(id) :		\
+	GICD_##REG##RE + (uintptr_t)(id) - MIN_ESPI_ID)
+
+#define	GICD_OFFSET(REG, id)						\
+	(((id) <= MAX_SPI_ID) ?						\
+	GICD_##REG##R + (((uintptr_t)(id) >> REG##R_SHIFT) << 2) :	\
+	GICD_##REG##RE + ((((uintptr_t)(id) - MIN_ESPI_ID) >>		\
+					REG##R_SHIFT) << 2))
+
+#define	GICD_OFFSET_64(REG, id)						\
+	(((id) <= MAX_SPI_ID) ?						\
+	GICD_##REG##R + (((uintptr_t)(id) >> REG##R_SHIFT) << 3) :	\
+	GICD_##REG##RE + ((((uintptr_t)(id) - MIN_ESPI_ID) >>		\
+					REG##R_SHIFT) << 3))
+
+#else	/* GICv3 */
+#define	GICD_OFFSET_8(REG, id)	\
+	(GICD_##REG##R + (uintptr_t)(id))
+
+#define	GICD_OFFSET(REG, id)	\
+	(GICD_##REG##R + (((uintptr_t)(id) >> REG##R_SHIFT) << 2))
+
+#define	GICD_OFFSET_64(REG, id)	\
+	(GICD_##REG##R + (((uintptr_t)(id) >> REG##R_SHIFT) << 3))
+#endif	/* GIC_EXT_INTID */
+
+/*
+ * Read/Write 8, 32 and 64-bit GIC Distributor register
+ * corresponding to its interrupt ID
+ */
+#define GICD_READ(REG, base, id)	\
+	mmio_read_32((base) + GICD_OFFSET(REG, (id)))
+
+#define GICD_READ_64(REG, base, id)	\
+	mmio_read_64((base) + GICD_OFFSET_64(REG, (id)))
+
+#define GICD_WRITE_8(REG, base, id, val)	\
+	mmio_write_8((base) + GICD_OFFSET_8(REG, (id)), (val))
+
+#define GICD_WRITE(REG, base, id, val)	\
+	mmio_write_32((base) + GICD_OFFSET(REG, (id)), (val))
+
+#define GICD_WRITE_64(REG, base, id, val)	\
+	mmio_write_64((base) + GICD_OFFSET_64(REG, (id)), (val))
+
+/*
+ * Bit operations on GIC Distributor register corresponding
+ * to its interrupt ID
+ */
+/* Get bit in GIC Distributor register */
+#define GICD_GET_BIT(REG, base, id)				\
+	((mmio_read_32((base) + GICD_OFFSET(REG, (id))) >>	\
+		BIT_NUM(REG, (id))) & 1U)
+
+/* Set bit in GIC Distributor register */
+#define GICD_SET_BIT(REG, base, id)				\
+	mmio_setbits_32((base) + GICD_OFFSET(REG, (id)),	\
+		((uint32_t)1 << BIT_NUM(REG, (id))))
+
+/* Clear bit in GIC Distributor register */
+#define GICD_CLR_BIT(REG, base, id)				\
+	mmio_clrbits_32((base) + GICD_OFFSET(REG, (id)),	\
+		((uint32_t)1 << BIT_NUM(REG, (id))))
+
+/* Write bit in GIC Distributor register */
+#define	GICD_WRITE_BIT(REG, base, id)			\
+	mmio_write_32((base) + GICD_OFFSET(REG, (id)),	\
+		((uint32_t)1 << BIT_NUM(REG, (id))))
+
+/*
+ * Calculate 8 and 32-bit GICR register offset
+ * corresponding to its interrupt ID
+ */
+#if GIC_EXT_INTID
+	/* GICv3.1 */
+#define	GICR_OFFSET_8(REG, id)				\
+	(((id) <= MAX_PPI_ID) ?				\
+	GICR_##REG##R + (uintptr_t)(id) :		\
+	GICR_##REG##R + (uintptr_t)(id) - (MIN_EPPI_ID - MIN_SPI_ID))
+
+#define GICR_OFFSET(REG, id)						\
+	(((id) <= MAX_PPI_ID) ?						\
+	GICR_##REG##R + (((uintptr_t)(id) >> REG##R_SHIFT) << 2) :	\
+	GICR_##REG##R + ((((uintptr_t)(id) - (MIN_EPPI_ID - MIN_SPI_ID))\
+						>> REG##R_SHIFT) << 2))
+#else	/* GICv3 */
+#define	GICR_OFFSET_8(REG, id)	\
+	(GICR_##REG##R + (uintptr_t)(id))
+
+#define GICR_OFFSET(REG, id)	\
+	(GICR_##REG##R + (((uintptr_t)(id) >> REG##R_SHIFT) << 2))
+#endif /* GIC_EXT_INTID */
+
+/* Read/Write GIC Redistributor register corresponding to its interrupt ID */
+#define GICR_READ(REG, base, id)			\
+	mmio_read_32((base) + GICR_OFFSET(REG, (id)))
+
+#define GICR_WRITE_8(REG, base, id, val)		\
+	mmio_write_8((base) + GICR_OFFSET_8(REG, (id)), (val))
+
+#define GICR_WRITE(REG, base, id, val)			\
+	mmio_write_32((base) + GICR_OFFSET(REG, (id)), (val))
+
+/*
+ * Bit operations on GIC Redistributor register
+ * corresponding to its interrupt ID
+ */
+/* Get bit in GIC Redistributor register */
+#define GICR_GET_BIT(REG, base, id)				\
+	((mmio_read_32((base) + GICR_OFFSET(REG, (id))) >>	\
+		BIT_NUM(REG, (id))) & 1U)
+
+/* Write bit in GIC Redistributor register */
+#define	GICR_WRITE_BIT(REG, base, id)				\
+	mmio_write_32((base) + GICR_OFFSET(REG, (id)),		\
+		((uint32_t)1 << BIT_NUM(REG, (id))))
+
+/* Set bit in GIC Redistributor register */
+#define	GICR_SET_BIT(REG, base, id)				\
+	mmio_setbits_32((base) + GICR_OFFSET(REG, (id)),	\
+		((uint32_t)1 << BIT_NUM(REG, (id))))
+
+/* Clear bit in GIC Redistributor register */
+#define	GICR_CLR_BIT(REG, base, id)				\
+	mmio_clrbits_32((base) + GICR_OFFSET(REG, (id)),	\
+		((uint32_t)1 << BIT_NUM(REG, (id))))
+
+/*
+ * Macro to convert an mpidr to a value suitable for programming into a
+ * GICD_IROUTER. Bits[31:24] in the MPIDR are cleared as they are not relevant
+ * to GICv3.
+ */
+static inline u_register_t gicd_irouter_val_from_mpidr(u_register_t mpidr,
+						       unsigned int irm)
+{
+	return (mpidr & MPIDR_AFFINITY_MASK) |
+		((irm & IROUTER_IRM_MASK) << IROUTER_IRM_SHIFT);
+}
+
+/*
+ * Macro to convert a GICR_TYPER affinity value into a MPIDR value. Bits[31:24]
+ * are zeroes.
+ */
+#ifdef __aarch64__
+static inline u_register_t mpidr_from_gicr_typer(uint64_t typer_val)
+{
+	return (((typer_val >> 56) & MPIDR_AFFLVL_MASK) << MPIDR_AFF3_SHIFT) |
+		((typer_val >> 32) & U(0xffffff));
+}
+#else
+static inline u_register_t mpidr_from_gicr_typer(uint64_t typer_val)
+{
+	return (((typer_val) >> 32) & U(0xffffff));
+}
+#endif
+
+/*******************************************************************************
+ * GICv3 private global variables declarations
+ ******************************************************************************/
+extern const gicv3_driver_data_t *gicv3_driver_data;
+
+/*******************************************************************************
+ * Private GICv3 function prototypes for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
+ ******************************************************************************/
+unsigned int gicd_read_igrpmodr(uintptr_t base, unsigned int id);
+unsigned int gicr_read_ipriorityr(uintptr_t base, unsigned int id);
+void gicd_write_igrpmodr(uintptr_t base, unsigned int id, unsigned int val);
+void gicr_write_ipriorityr(uintptr_t base, unsigned int id, unsigned int val);
+
+/*******************************************************************************
+ * Private GICv3 function prototypes for accessing the GIC registers
+ * corresponding to a single interrupt ID. These functions use bitwise
+ * operations or appropriate register accesses to modify or return
+ * the bit-field corresponding the single interrupt ID.
+ ******************************************************************************/
+unsigned int gicd_get_igrpmodr(uintptr_t base, unsigned int id);
+unsigned int gicr_get_igrpmodr(uintptr_t base, unsigned int id);
+unsigned int gicr_get_igroupr(uintptr_t base, unsigned int id);
+unsigned int gicr_get_isactiver(uintptr_t base, unsigned int id);
+void gicd_set_igrpmodr(uintptr_t base, unsigned int id);
+void gicr_set_igrpmodr(uintptr_t base, unsigned int id);
+void gicr_set_isenabler(uintptr_t base, unsigned int id);
+void gicr_set_icenabler(uintptr_t base, unsigned int id);
+void gicr_set_ispendr(uintptr_t base, unsigned int id);
+void gicr_set_icpendr(uintptr_t base, unsigned int id);
+void gicr_set_igroupr(uintptr_t base, unsigned int id);
+void gicd_clr_igrpmodr(uintptr_t base, unsigned int id);
+void gicr_clr_igrpmodr(uintptr_t base, unsigned int id);
+void gicr_clr_igroupr(uintptr_t base, unsigned int id);
+void gicr_set_ipriorityr(uintptr_t base, unsigned int id, unsigned int pri);
+void gicr_set_icfgr(uintptr_t base, unsigned int id, unsigned int cfg);
+
+/*******************************************************************************
+ * Private GICv3 helper function prototypes
+ ******************************************************************************/
+unsigned int gicv3_get_spi_limit(uintptr_t gicd_base);
+unsigned int gicv3_get_espi_limit(uintptr_t gicd_base);
+void gicv3_spis_config_defaults(uintptr_t gicd_base);
+void gicv3_ppi_sgi_config_defaults(uintptr_t gicr_base);
+unsigned int gicv3_secure_ppi_sgi_config_props(uintptr_t gicr_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num);
+unsigned int gicv3_secure_spis_config_props(uintptr_t gicd_base,
+		const interrupt_prop_t *interrupt_props,
+		unsigned int interrupt_props_num);
+void gicv3_rdistif_base_addrs_probe(uintptr_t *rdistif_base_addrs,
+					unsigned int rdistif_num,
+					uintptr_t gicr_base,
+					mpidr_hash_fn mpidr_to_core_pos);
+void gicv3_rdistif_mark_core_awake(uintptr_t gicr_base);
+void gicv3_rdistif_mark_core_asleep(uintptr_t gicr_base);
+
+/*******************************************************************************
+ * GIC Distributor interface accessors
+ ******************************************************************************/
+/*
+ * Wait for updates to:
+ * GICD_CTLR[2:0] - the Group Enables
+ * GICD_CTLR[7:4] - the ARE bits, E1NWF bit and DS bit
+ * GICD_ICENABLER<n> - the clearing of enable state for SPIs
+ */
+static inline void gicd_wait_for_pending_write(uintptr_t gicd_base)
+{
+	while ((gicd_read_ctlr(gicd_base) & GICD_CTLR_RWP_BIT) != 0U) {
+	}
+}
+
+static inline uint32_t gicd_read_pidr2(uintptr_t base)
+{
+	return mmio_read_32(base + GICD_PIDR2_GICV3);
+}
+
+static inline uint64_t gicd_read_irouter(uintptr_t base, unsigned int id)
+{
+	assert(id >= MIN_SPI_ID);
+	return GICD_READ_64(IROUTE, base, id);
+}
+
+static inline void gicd_write_irouter(uintptr_t base,
+				      unsigned int id,
+				      uint64_t affinity)
+{
+	assert(id >= MIN_SPI_ID);
+	GICD_WRITE_64(IROUTE, base, id, affinity);
+}
+
+static inline void gicd_clr_ctlr(uintptr_t base,
+				 unsigned int bitmap,
+				 unsigned int rwp)
+{
+	gicd_write_ctlr(base, gicd_read_ctlr(base) & ~bitmap);
+	if (rwp != 0U) {
+		gicd_wait_for_pending_write(base);
+	}
+}
+
+static inline void gicd_set_ctlr(uintptr_t base,
+				 unsigned int bitmap,
+				 unsigned int rwp)
+{
+	gicd_write_ctlr(base, gicd_read_ctlr(base) | bitmap);
+	if (rwp != 0U) {
+		gicd_wait_for_pending_write(base);
+	}
+}
+
+/*******************************************************************************
+ * GIC Redistributor interface accessors
+ ******************************************************************************/
+static inline uint32_t gicr_read_ctlr(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_CTLR);
+}
+
+static inline void gicr_write_ctlr(uintptr_t base, uint32_t val)
+{
+	mmio_write_32(base + GICR_CTLR, val);
+}
+
+static inline uint64_t gicr_read_typer(uintptr_t base)
+{
+	return mmio_read_64(base + GICR_TYPER);
+}
+
+static inline uint32_t gicr_read_waker(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_WAKER);
+}
+
+static inline void gicr_write_waker(uintptr_t base, uint32_t val)
+{
+	mmio_write_32(base + GICR_WAKER, val);
+}
+
+/*
+ * Wait for updates to:
+ * GICR_ICENABLER0
+ * GICR_CTLR.DPG1S
+ * GICR_CTLR.DPG1NS
+ * GICR_CTLR.DPG0
+ * GICR_CTLR, which clears EnableLPIs from 1 to 0
+ */
+static inline void gicr_wait_for_pending_write(uintptr_t gicr_base)
+{
+	while ((gicr_read_ctlr(gicr_base) & GICR_CTLR_RWP_BIT) != 0U) {
+	}
+}
+
+static inline void gicr_wait_for_upstream_pending_write(uintptr_t gicr_base)
+{
+	while ((gicr_read_ctlr(gicr_base) & GICR_CTLR_UWP_BIT) != 0U) {
+	}
+}
+
+/* Private implementation of Distributor power control hooks */
+void arm_gicv3_distif_pre_save(unsigned int rdist_proc_num);
+void arm_gicv3_distif_post_restore(unsigned int rdist_proc_num);
+
+/*******************************************************************************
+ * GIC Redistributor functions for accessing entire registers.
+ * Note: The raw register values correspond to multiple interrupt IDs and
+ * the number of interrupt IDs involved depends on the register accessed.
+ ******************************************************************************/
+
+/*
+ * Accessors to read/write GIC Redistributor ICENABLER0 register
+ */
+static inline unsigned int gicr_read_icenabler0(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_ICENABLER0);
+}
+
+static inline void gicr_write_icenabler0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_ICENABLER0, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ICENABLER0 and ICENABLERE
+ * register corresponding to its number
+ */
+static inline unsigned int gicr_read_icenabler(uintptr_t base,
+						unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_ICENABLER + (reg_num << 2));
+}
+
+static inline void gicr_write_icenabler(uintptr_t base, unsigned int reg_num,
+					unsigned int val)
+{
+	mmio_write_32(base + GICR_ICENABLER + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ICFGR0, ICFGR1 registers
+ */
+static inline unsigned int gicr_read_icfgr0(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_ICFGR0);
+}
+
+static inline unsigned int gicr_read_icfgr1(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_ICFGR1);
+}
+
+static inline void gicr_write_icfgr0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_ICFGR0, val);
+}
+
+static inline void gicr_write_icfgr1(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_ICFGR1, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ICFGR0, ICFGR1 and ICFGRE
+ * register corresponding to its number
+ */
+static inline unsigned int gicr_read_icfgr(uintptr_t base, unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_ICFGR + (reg_num << 2));
+}
+
+static inline void gicr_write_icfgr(uintptr_t base, unsigned int reg_num,
+					unsigned int val)
+{
+	mmio_write_32(base + GICR_ICFGR + (reg_num << 2), val);
+}
+
+/*
+ * Accessor to write GIC Redistributor ICPENDR0 register
+ */
+static inline void gicr_write_icpendr0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_ICPENDR0, val);
+}
+
+/*
+ * Accessor to write GIC Redistributor ICPENDR0 and ICPENDRE
+ * register corresponding to its number
+ */
+static inline void gicr_write_icpendr(uintptr_t base, unsigned int reg_num,
+					unsigned int val)
+{
+	mmio_write_32(base + GICR_ICPENDR + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor IGROUPR0 register
+ */
+static inline unsigned int gicr_read_igroupr0(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_IGROUPR0);
+}
+
+static inline void gicr_write_igroupr0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_IGROUPR0, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor IGROUPR0 and IGROUPRE
+ * register corresponding to its number
+ */
+static inline unsigned int gicr_read_igroupr(uintptr_t base,
+						unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_IGROUPR + (reg_num << 2));
+}
+
+static inline void gicr_write_igroupr(uintptr_t base, unsigned int reg_num,
+						unsigned int val)
+{
+	mmio_write_32(base + GICR_IGROUPR + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor IGRPMODR0 register
+ */
+static inline unsigned int gicr_read_igrpmodr0(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_IGRPMODR0);
+}
+
+static inline void gicr_write_igrpmodr0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_IGRPMODR0, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor IGRPMODR0 and IGRPMODRE
+ * register corresponding to its number
+ */
+static inline unsigned int gicr_read_igrpmodr(uintptr_t base,
+						unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_IGRPMODR + (reg_num << 2));
+}
+
+static inline void gicr_write_igrpmodr(uintptr_t base, unsigned int reg_num,
+				       unsigned int val)
+{
+	mmio_write_32(base + GICR_IGRPMODR + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write the GIC Redistributor IPRIORITYR(E) register
+ * corresponding to its number, 4 interrupts IDs at a time.
+ */
+static inline unsigned int gicr_ipriorityr_read(uintptr_t base,
+						unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_IPRIORITYR + (reg_num << 2));
+}
+
+static inline void gicr_ipriorityr_write(uintptr_t base, unsigned int reg_num,
+						unsigned int val)
+{
+	mmio_write_32(base + GICR_IPRIORITYR + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ISACTIVER0 register
+ */
+static inline unsigned int gicr_read_isactiver0(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_ISACTIVER0);
+}
+
+static inline void gicr_write_isactiver0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_ISACTIVER0, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ISACTIVER0 and ISACTIVERE
+ * register corresponding to its number
+ */
+static inline unsigned int gicr_read_isactiver(uintptr_t base,
+						unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_ISACTIVER + (reg_num << 2));
+}
+
+static inline void gicr_write_isactiver(uintptr_t base, unsigned int reg_num,
+					unsigned int val)
+{
+	mmio_write_32(base + GICR_ISACTIVER + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ISENABLER0 register
+ */
+static inline unsigned int gicr_read_isenabler0(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_ISENABLER0);
+}
+
+static inline void gicr_write_isenabler0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_ISENABLER0, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ISENABLER0 and ISENABLERE
+ * register corresponding to its number
+ */
+static inline unsigned int gicr_read_isenabler(uintptr_t base,
+						unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_ISENABLER + (reg_num << 2));
+}
+
+static inline void gicr_write_isenabler(uintptr_t base, unsigned int reg_num,
+					unsigned int val)
+{
+	mmio_write_32(base + GICR_ISENABLER + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ISPENDR0 register
+ */
+static inline unsigned int gicr_read_ispendr0(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_ISPENDR0);
+}
+
+static inline void gicr_write_ispendr0(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_ISPENDR0, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor ISPENDR0 and ISPENDRE
+ * register corresponding to its number
+ */
+static inline unsigned int gicr_read_ispendr(uintptr_t base,
+						unsigned int reg_num)
+{
+	return mmio_read_32(base + GICR_ISPENDR + (reg_num << 2));
+}
+
+static inline void gicr_write_ispendr(uintptr_t base, unsigned int reg_num,
+						unsigned int val)
+{
+	mmio_write_32(base + GICR_ISPENDR + (reg_num << 2), val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor NSACR register
+ */
+static inline unsigned int gicr_read_nsacr(uintptr_t base)
+{
+	return mmio_read_32(base + GICR_NSACR);
+}
+
+static inline void gicr_write_nsacr(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GICR_NSACR, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor PROPBASER register
+ */
+static inline uint64_t gicr_read_propbaser(uintptr_t base)
+{
+	return mmio_read_64(base + GICR_PROPBASER);
+}
+
+static inline void gicr_write_propbaser(uintptr_t base, uint64_t val)
+{
+	mmio_write_64(base + GICR_PROPBASER, val);
+}
+
+/*
+ * Accessors to read/write GIC Redistributor PENDBASER register
+ */
+static inline uint64_t gicr_read_pendbaser(uintptr_t base)
+{
+	return mmio_read_64(base + GICR_PENDBASER);
+}
+
+static inline void gicr_write_pendbaser(uintptr_t base, uint64_t val)
+{
+	mmio_write_64(base + GICR_PENDBASER, val);
+}
+
+/*******************************************************************************
+ * GIC ITS functions to read and write entire ITS registers.
+ ******************************************************************************/
+static inline uint32_t gits_read_ctlr(uintptr_t base)
+{
+	return mmio_read_32(base + GITS_CTLR);
+}
+
+static inline void gits_write_ctlr(uintptr_t base, uint32_t val)
+{
+	mmio_write_32(base + GITS_CTLR, val);
+}
+
+static inline uint64_t gits_read_cbaser(uintptr_t base)
+{
+	return mmio_read_64(base + GITS_CBASER);
+}
+
+static inline void gits_write_cbaser(uintptr_t base, uint64_t val)
+{
+	mmio_write_64(base + GITS_CBASER, val);
+}
+
+static inline uint64_t gits_read_cwriter(uintptr_t base)
+{
+	return mmio_read_64(base + GITS_CWRITER);
+}
+
+static inline void gits_write_cwriter(uintptr_t base, uint64_t val)
+{
+	mmio_write_64(base + GITS_CWRITER, val);
+}
+
+static inline uint64_t gits_read_baser(uintptr_t base,
+					unsigned int its_table_id)
+{
+	assert(its_table_id < 8U);
+	return mmio_read_64(base + GITS_BASER + (8U * its_table_id));
+}
+
+static inline void gits_write_baser(uintptr_t base, unsigned int its_table_id,
+					uint64_t val)
+{
+	assert(its_table_id < 8U);
+	mmio_write_64(base + GITS_BASER + (8U * its_table_id), val);
+}
+
+/*
+ * Wait for Quiescent bit when GIC ITS is disabled
+ */
+static inline void gits_wait_for_quiescent_bit(uintptr_t gits_base)
+{
+	assert((gits_read_ctlr(gits_base) & GITS_CTLR_ENABLED_BIT) == 0U);
+	while ((gits_read_ctlr(gits_base) & GITS_CTLR_QUIESCENT_BIT) == 0U) {
+	}
+}
+
+#endif /* GICV3_PRIVATE_H */
diff --git a/drivers/arm/mhu/mhu_v2_x.c b/drivers/arm/mhu/mhu_v2_x.c
new file mode 100644
index 0000000..3103b92
--- /dev/null
+++ b/drivers/arm/mhu/mhu_v2_x.c
@@ -0,0 +1,379 @@
+/*
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdbool.h>
+#include <stdint.h>
+
+#include "mhu_v2_x.h"
+
+#define MHU_V2_X_MAX_CHANNELS		124
+#define MHU_V2_1_MAX_CHCOMB_INT		4
+#define ENABLE				0x1
+#define DISABLE				0x0
+#define CLEAR_INTR			0x1
+#define CH_PER_CH_COMB			0x20
+#define SEND_FRAME(p_mhu)		((struct mhu_v2_x_send_frame_t *)p_mhu)
+#define RECV_FRAME(p_mhu)		((struct mhu_v2_x_recv_frame_t *)p_mhu)
+
+#define MHU_MAJOR_REV_V2		0x1u
+#define MHU_MINOR_REV_2_0		0x0u
+#define MHU_MINOR_REV_2_1		0x1u
+
+struct mhu_v2_x_send_ch_window_t {
+	/* Offset: 0x00 (R/ ) Channel Status */
+	volatile uint32_t ch_st;
+	/* Offset: 0x04 (R/ ) Reserved */
+	volatile uint32_t reserved_0;
+	/* Offset: 0x08 (R/ ) Reserved */
+	volatile uint32_t reserved_1;
+	/* Offset: 0x0C ( /W) Channel Set */
+	volatile uint32_t ch_set;
+	/* Offset: 0x10 (R/ ) Channel Interrupt Status (Reserved in 2.0) */
+	volatile uint32_t ch_int_st;
+	/* Offset: 0x14 ( /W) Channel Interrupt Clear  (Reserved in 2.0) */
+	volatile uint32_t ch_int_clr;
+	/* Offset: 0x18 (R/W) Channel Interrupt Enable (Reserved in 2.0) */
+	volatile uint32_t ch_int_en;
+	/* Offset: 0x1C (R/ ) Reserved */
+	volatile uint32_t reserved_2;
+};
+
+struct mhu_v2_x_send_frame_t {
+	/* Offset: 0x000 ( / ) Sender Channel Window 0 -123 */
+	struct mhu_v2_x_send_ch_window_t send_ch_window[MHU_V2_X_MAX_CHANNELS];
+	/* Offset: 0xF80 (R/ ) Message Handling Unit Configuration */
+	volatile uint32_t mhu_cfg;
+	/* Offset: 0xF84 (R/W) Response Configuration */
+	volatile uint32_t resp_cfg;
+	/* Offset: 0xF88 (R/W) Access Request */
+	volatile uint32_t access_request;
+	/* Offset: 0xF8C (R/ ) Access Ready */
+	volatile uint32_t access_ready;
+	/* Offset: 0xF90 (R/ ) Interrupt Status */
+	volatile uint32_t int_st;
+	/* Offset: 0xF94 ( /W) Interrupt Clear */
+	volatile uint32_t int_clr;
+	/* Offset: 0xF98 (R/W) Interrupt Enable */
+	volatile uint32_t int_en;
+	/* Offset: 0xF9C (R/ ) Reserved */
+	volatile uint32_t reserved_0;
+	/* Offset: 0xFA0 (R/W) Channel Combined IRQ Stat (Reserved in 2.0) */
+	volatile uint32_t ch_comb_int_st[MHU_V2_1_MAX_CHCOMB_INT];
+	/* Offset: 0xFC4 (R/ ) Reserved */
+	volatile uint32_t reserved_1[6];
+	/* Offset: 0xFC8 (R/ ) Implementer Identification Register */
+	volatile uint32_t iidr;
+	/* Offset: 0xFCC (R/ ) Architecture Identification Register */
+	volatile uint32_t aidr;
+	/* Offset: 0xFD0 (R/ )  */
+	volatile uint32_t pid_1[4];
+	/* Offset: 0xFE0 (R/ )  */
+	volatile uint32_t pid_0[4];
+	/* Offset: 0xFF0 (R/ )  */
+	volatile uint32_t cid[4];
+};
+
+struct mhu_v2_x_rec_ch_window_t {
+	/* Offset: 0x00 (R/ ) Channel Status */
+	volatile uint32_t ch_st;
+	/* Offset: 0x04 (R/ ) Channel Status Masked */
+	volatile uint32_t ch_st_msk;
+	/* Offset: 0x08 ( /W) Channel Clear */
+	volatile uint32_t ch_clr;
+	/* Offset: 0x0C (R/ ) Reserved */
+	volatile uint32_t reserved_0;
+	/* Offset: 0x10 (R/ ) Channel Mask Status */
+	volatile uint32_t ch_msk_st;
+	/* Offset: 0x14 ( /W) Channel Mask Set */
+	volatile uint32_t ch_msk_set;
+	/* Offset: 0x18 ( /W) Channel Mask Clear */
+	volatile uint32_t ch_msk_clr;
+	/* Offset: 0x1C (R/ ) Reserved */
+	volatile uint32_t reserved_1;
+};
+
+struct mhu_v2_x_recv_frame_t {
+	/* Offset: 0x000 ( / ) Receiver Channel Window 0 -123 */
+	struct mhu_v2_x_rec_ch_window_t rec_ch_window[MHU_V2_X_MAX_CHANNELS];
+	/* Offset: 0xF80 (R/ ) Message Handling Unit Configuration */
+	volatile uint32_t mhu_cfg;
+	/* Offset: 0xF84 (R/ ) Reserved */
+	volatile uint32_t reserved_0[3];
+	/* Offset: 0xF90 (R/ ) Interrupt Status (Reserved in 2.0) */
+	volatile uint32_t int_st;
+	/* Offset: 0xF94 (R/ ) Interrupt Clear  (Reserved in 2.0) */
+	volatile uint32_t int_clr;
+	/* Offset: 0xF98 (R/W) Interrupt Enable (Reserved in 2.0) */
+	volatile uint32_t int_en;
+	/* Offset: 0xF9C (R/ ) Reserved  */
+	volatile uint32_t reserved_1;
+	/* Offset: 0xFA0 (R/ ) Channel Combined IRQ Stat (Reserved in 2.0) */
+	volatile uint32_t ch_comb_int_st[MHU_V2_1_MAX_CHCOMB_INT];
+	/* Offset: 0xFB0 (R/ ) Reserved */
+	volatile uint32_t reserved_2[6];
+	/* Offset: 0xFC8 (R/ ) Implementer Identification Register */
+	volatile uint32_t iidr;
+	/* Offset: 0xFCC (R/ ) Architecture Identification Register */
+	volatile uint32_t aidr;
+	/* Offset: 0xFD0 (R/ )  */
+	volatile uint32_t pid_1[4];
+	/* Offset: 0xFE0 (R/ )  */
+	volatile uint32_t pid_0[4];
+	/* Offset: 0xFF0 (R/ )  */
+	volatile uint32_t cid[4];
+};
+
+union mhu_v2_x_frame {
+	struct mhu_v2_x_send_frame_t send_frame;
+	struct mhu_v2_x_recv_frame_t recv_frame;
+};
+
+enum mhu_v2_x_error_t mhu_v2_x_driver_init(struct mhu_v2_x_dev_t *dev,
+	 enum mhu_v2_x_supported_revisions rev)
+{
+	uint32_t AIDR = 0;
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (dev->is_initialized) {
+		return MHU_V_2_X_ERR_ALREADY_INIT;
+	}
+
+	if (rev == MHU_REV_READ_FROM_HW) {
+		/* Read revision from HW */
+		if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+			AIDR = p_mhu->recv_frame.aidr;
+		} else {
+			AIDR = p_mhu->send_frame.aidr;
+		}
+
+		/* Get bits 7:4 to read major revision */
+		if (((AIDR >> 4) & 0b1111) != MHU_MAJOR_REV_V2) {
+			/* Unsupported MHU version */
+			return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
+		} /* No need to save major version, driver only supports MHUv2 */
+
+		/* Get bits 3:0 to read minor revision */
+		dev->subversion = AIDR & 0b1111;
+
+		if (dev->subversion != MHU_MINOR_REV_2_0 &&
+			dev->subversion != MHU_MINOR_REV_2_1) {
+			/* Unsupported subversion */
+			return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
+		}
+	} else {
+		/* Revisions were provided by caller */
+		if (rev == MHU_REV_2_0) {
+			dev->subversion = MHU_MINOR_REV_2_0;
+		} else if (rev == MHU_REV_2_1) {
+			dev->subversion = MHU_MINOR_REV_2_1;
+		} else {
+			/* Unsupported subversion */
+			return MHU_V_2_X_ERR_UNSUPPORTED_VERSION;
+		} /* No need to save major version, driver only supports MHUv2 */
+	}
+
+	dev->is_initialized = true;
+
+	return MHU_V_2_X_ERR_NONE;
+}
+
+uint32_t mhu_v2_x_get_num_channel_implemented(const struct mhu_v2_x_dev_t *dev)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_SENDER_FRAME) {
+		return (SEND_FRAME(p_mhu))->mhu_cfg;
+	} else {
+		assert(dev->frame == MHU_V2_X_RECEIVER_FRAME);
+		return (RECV_FRAME(p_mhu))->mhu_cfg;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_send(const struct mhu_v2_x_dev_t *dev,
+	 uint32_t channel, uint32_t val)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_SENDER_FRAME) {
+		(SEND_FRAME(p_mhu))->send_ch_window[channel].ch_set = val;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_poll(const struct mhu_v2_x_dev_t *dev,
+	 uint32_t channel, uint32_t *value)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_SENDER_FRAME) {
+		*value = (SEND_FRAME(p_mhu))->send_ch_window[channel].ch_st;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_clear(const struct mhu_v2_x_dev_t *dev,
+	 uint32_t channel)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		(RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_clr = UINT32_MAX;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_receive(
+	 const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t *value)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		*value = (RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_st;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_set(
+	 const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		(RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_msk_set = mask;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_clear(
+	 const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame == MHU_V2_X_RECEIVER_FRAME) {
+		(RECV_FRAME(p_mhu))->rec_ch_window[channel].ch_msk_clr = mask;
+		return MHU_V_2_X_ERR_NONE;
+	} else {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+}
+enum mhu_v2_x_error_t mhu_v2_x_initiate_transfer(
+	 const struct mhu_v2_x_dev_t *dev)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame != MHU_V2_X_SENDER_FRAME) {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+
+	(SEND_FRAME(p_mhu))->access_request = ENABLE;
+
+	while (!((SEND_FRAME(p_mhu))->access_ready)) {
+		/* Wait in a loop for access ready signal to be high */
+		;
+	}
+
+	return MHU_V_2_X_ERR_NONE;
+}
+
+enum mhu_v2_x_error_t mhu_v2_x_close_transfer(const struct mhu_v2_x_dev_t *dev)
+{
+	union mhu_v2_x_frame *p_mhu;
+
+	assert(dev != NULL);
+
+	p_mhu = (union mhu_v2_x_frame *)dev->base;
+
+	if (!(dev->is_initialized)) {
+		return MHU_V_2_X_ERR_NOT_INIT;
+	}
+
+	if (dev->frame != MHU_V2_X_SENDER_FRAME) {
+		return MHU_V_2_X_ERR_INVALID_ARG;
+	}
+
+	(SEND_FRAME(p_mhu))->access_request = DISABLE;
+
+	return MHU_V_2_X_ERR_NONE;
+}
diff --git a/drivers/arm/mhu/mhu_v2_x.h b/drivers/arm/mhu/mhu_v2_x.h
new file mode 100644
index 0000000..10247d2
--- /dev/null
+++ b/drivers/arm/mhu/mhu_v2_x.h
@@ -0,0 +1,210 @@
+/*
+ * Copyright (c) 2020-2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef MHU_V2_X_H
+#define MHU_V2_X_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+#define MHU_2_X_INTR_NR2R_OFF		(0x0u)
+#define MHU_2_X_INTR_R2NR_OFF		(0x1u)
+#define MHU_2_1_INTR_CHCOMB_OFF		(0x2u)
+
+#define MHU_2_X_INTR_NR2R_MASK		(0x1u << MHU_2_X_INTR_NR2R_OFF)
+#define MHU_2_X_INTR_R2NR_MASK		(0x1u << MHU_2_X_INTR_R2NR_OFF)
+#define MHU_2_1_INTR_CHCOMB_MASK	(0x1u << MHU_2_1_INTR_CHCOMB_OFF)
+
+enum mhu_v2_x_frame_t {
+	MHU_V2_X_SENDER_FRAME   = 0x0u,
+	MHU_V2_X_RECEIVER_FRAME = 0x1u,
+};
+
+enum mhu_v2_x_supported_revisions {
+	MHU_REV_READ_FROM_HW = 0,
+	MHU_REV_2_0,
+	MHU_REV_2_1,
+};
+
+struct mhu_v2_x_dev_t {
+	uintptr_t base;
+	enum mhu_v2_x_frame_t frame;
+	uint32_t subversion;	/*!< Hardware subversion: v2.X */
+	bool is_initialized;	/*!< Indicates if the MHU driver
+				 *   is initialized and enabled
+				 */
+};
+
+/**
+ * MHU v2 error enumeration types.
+ */
+enum mhu_v2_x_error_t {
+	MHU_V_2_X_ERR_NONE			=  0,
+	MHU_V_2_X_ERR_NOT_INIT			= -1,
+	MHU_V_2_X_ERR_ALREADY_INIT		= -2,
+	MHU_V_2_X_ERR_UNSUPPORTED_VERSION	= -3,
+	MHU_V_2_X_ERR_INVALID_ARG		= -4,
+	MHU_V_2_X_ERR_GENERAL			= -5
+};
+
+/**
+ * Initializes the driver.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * rev		MHU revision (if can't be identified from HW).
+ *
+ * Reads the MHU hardware version.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * MHU revision only has to be specified when versions can't be read
+ * from HW (ARCH_MAJOR_REV reg reads as 0x0).
+ *
+ * This function doesn't check if dev is NULL.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_driver_init(struct mhu_v2_x_dev_t *dev,
+	enum mhu_v2_x_supported_revisions rev);
+
+/**
+ * Returns the number of channels implemented.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ *
+ * This function doesn't check if dev is NULL.
+ */
+uint32_t mhu_v2_x_get_num_channel_implemented(
+		const struct mhu_v2_x_dev_t *dev);
+
+/**
+ * Sends the value over a channel.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to send the value over.
+ * val		Value to send.
+ *
+ * Sends the value over a channel.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_send(const struct mhu_v2_x_dev_t *dev,
+	uint32_t channel, uint32_t val);
+
+/**
+ * Polls sender channel status.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to poll the status of.
+ * value	Pointer to variable that will store the value.
+ *
+ * Polls sender channel status.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_poll(const struct mhu_v2_x_dev_t *dev,
+	uint32_t channel, uint32_t *value);
+
+/**
+ * Clears the channel after the value is send over it.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to clear.
+ *
+ * Clears the channel after the value is send over it.
+ *
+ * Returns mhu_v2_x_error_t error code..
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_clear(const struct mhu_v2_x_dev_t *dev,
+	uint32_t channel);
+
+/**
+ * Receives the value over a channel.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Channel to receive the value from.
+ * value	Pointer to variable that will store the value.
+ *
+ * Receives the value over a channel.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ * This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_receive(
+	const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t *value);
+
+/**
+ * Sets bits in the Channel Mask.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ * channel	Which channel's mask to set.
+ * mask		Mask to be set over a receiver frame.
+ *
+ * Sets bits in the Channel Mask.
+ *
+ * Returns mhu_v2_x_error_t error code..
+ *
+ * This function doesn't check if dev is NULL.
+ *  This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_set(
+	const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask);
+
+/**
+ * Clears bits in the Channel Mask.
+ *
+ * dev	MHU device struct mhu_v2_x_dev_t.
+ * channel	Which channel's mask to clear.
+ * mask	Mask to be clear over a receiver frame.
+ *
+ * Clears bits in the Channel Mask.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ *  This function doesn't check if channel is implemented.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_channel_mask_clear(
+	const struct mhu_v2_x_dev_t *dev, uint32_t channel, uint32_t mask);
+
+/**
+ * Initiates a MHU transfer with the handshake signals.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ *
+ * Initiates a MHU transfer with the handshake signals in a blocking mode.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_initiate_transfer(
+	const struct mhu_v2_x_dev_t *dev);
+
+/**
+ * Closes a MHU transfer with the handshake signals.
+ *
+ * dev		MHU device struct mhu_v2_x_dev_t.
+ *
+ * Closes a MHU transfer with the handshake signals in a blocking mode.
+ *
+ * Returns mhu_v2_x_error_t error code.
+ *
+ * This function doesn't check if dev is NULL.
+ */
+enum mhu_v2_x_error_t mhu_v2_x_close_transfer(
+	const struct mhu_v2_x_dev_t *dev);
+
+#endif /* MHU_V2_X_H */
diff --git a/drivers/arm/mhu/mhu_wrapper_v2_x.c b/drivers/arm/mhu/mhu_wrapper_v2_x.c
new file mode 100644
index 0000000..60de1d3
--- /dev/null
+++ b/drivers/arm/mhu/mhu_wrapper_v2_x.c
@@ -0,0 +1,312 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+#include <stdint.h>
+#include <string.h>
+
+#include <drivers/arm/mhu.h>
+
+#include "mhu_v2_x.h"
+
+#define MHU_NOTIFY_VALUE	(1234u)
+
+/*
+ * MHU devices for host:
+ * HSE: Host to Secure Enclave (sender device)
+ * SEH: Secure Enclave to Host (receiver device)
+ */
+struct mhu_v2_x_dev_t MHU1_HSE_DEV = {0, MHU_V2_X_SENDER_FRAME};
+struct mhu_v2_x_dev_t MHU1_SEH_DEV = {0, MHU_V2_X_RECEIVER_FRAME};
+
+static enum mhu_error_t error_mapping_to_mhu_error_t(enum mhu_v2_x_error_t err)
+{
+	switch (err) {
+	case MHU_V_2_X_ERR_NONE:
+		return MHU_ERR_NONE;
+	case MHU_V_2_X_ERR_NOT_INIT:
+		return MHU_ERR_NOT_INIT;
+	case MHU_V_2_X_ERR_ALREADY_INIT:
+		return MHU_ERR_ALREADY_INIT;
+	case MHU_V_2_X_ERR_UNSUPPORTED_VERSION:
+		return MHU_ERR_UNSUPPORTED_VERSION;
+	case MHU_V_2_X_ERR_INVALID_ARG:
+		return MHU_ERR_INVALID_ARG;
+	case MHU_V_2_X_ERR_GENERAL:
+		return MHU_ERR_GENERAL;
+	default:
+		return MHU_ERR_GENERAL;
+	}
+}
+
+static enum mhu_v2_x_error_t signal_and_wait_for_clear(void)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_HSE_DEV;
+	uint32_t val = MHU_NOTIFY_VALUE;
+	/* Using the last channel for notifications */
+	uint32_t channel_notify = mhu_v2_x_get_num_channel_implemented(dev) - 1;
+
+	err = mhu_v2_x_channel_send(dev, channel_notify, val);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return err;
+	}
+
+	do {
+		err = mhu_v2_x_channel_poll(dev, channel_notify, &val);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			break;
+		}
+	} while (val != 0);
+
+	return err;
+}
+
+static enum mhu_v2_x_error_t wait_for_signal(void)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_SEH_DEV;
+	uint32_t val = 0;
+	/* Using the last channel for notifications */
+	uint32_t channel_notify = mhu_v2_x_get_num_channel_implemented(dev) - 1;
+
+	do {
+		err = mhu_v2_x_channel_receive(dev, channel_notify, &val);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			break;
+		}
+	} while (val != MHU_NOTIFY_VALUE);
+
+	return err;
+}
+
+static enum mhu_v2_x_error_t clear_and_wait_for_next_signal(void)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_SEH_DEV;
+	uint32_t num_channels = mhu_v2_x_get_num_channel_implemented(dev);
+	uint32_t i;
+
+	/* Clear all channels */
+	for (i = 0; i < num_channels; ++i) {
+		err = mhu_v2_x_channel_clear(dev, i);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return err;
+		}
+	}
+
+	return wait_for_signal();
+}
+
+enum mhu_error_t mhu_init_sender(uintptr_t mhu_sender_base)
+{
+	enum mhu_v2_x_error_t err;
+
+	assert(mhu_sender_base != (uintptr_t)NULL);
+
+	MHU1_HSE_DEV.base = mhu_sender_base;
+
+	err = mhu_v2_x_driver_init(&MHU1_HSE_DEV, MHU_REV_READ_FROM_HW);
+	return error_mapping_to_mhu_error_t(err);
+}
+
+enum mhu_error_t mhu_init_receiver(uintptr_t mhu_receiver_base)
+{
+	enum mhu_v2_x_error_t err;
+	uint32_t num_channels, i;
+
+	assert(mhu_receiver_base != (uintptr_t)NULL);
+
+	MHU1_SEH_DEV.base = mhu_receiver_base;
+
+	err = mhu_v2_x_driver_init(&MHU1_SEH_DEV, MHU_REV_READ_FROM_HW);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+
+	num_channels = mhu_v2_x_get_num_channel_implemented(&MHU1_SEH_DEV);
+
+	/* Mask all channels except the notifying channel */
+	for (i = 0; i < (num_channels - 1); ++i) {
+		err = mhu_v2_x_channel_mask_set(&MHU1_SEH_DEV, i, UINT32_MAX);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+	}
+
+	/* The last channel is used for notifications */
+	err = mhu_v2_x_channel_mask_clear(
+		&MHU1_SEH_DEV, (num_channels - 1), UINT32_MAX);
+	return error_mapping_to_mhu_error_t(err);
+}
+
+/*
+ * Public function. See mhu.h
+ *
+ * The basic steps of transferring a message:
+ * 1.	Initiate MHU transfer.
+ * 2.	Send over the size of the payload on Channel 1. It is the very first
+ *	4 Bytes of the transfer. Continue with Channel 2.
+ * 3.	Send over the payload, writing the channels one after the other
+ *	(4 Bytes each). The last available channel is reserved for controlling
+ *	the transfer.
+ *	When the last channel is reached or no more data is left, STOP.
+ * 4.	Notify the receiver using the last channel and wait for acknowledge.
+ *	If there is still data to transfer, jump to step 3. Otherwise, proceed.
+ * 5.	Close MHU transfer.
+ *
+ */
+enum mhu_error_t mhu_send_data(const uint8_t *send_buffer, size_t size)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_HSE_DEV;
+	uint32_t num_channels = mhu_v2_x_get_num_channel_implemented(dev);
+	uint32_t chan = 0;
+	uint32_t i;
+	uint32_t *p;
+
+	/* For simplicity, require the send_buffer to be 4-byte aligned */
+	if ((uintptr_t)send_buffer & 0x3U) {
+		return MHU_ERR_INVALID_ARG;
+	}
+
+	err = mhu_v2_x_initiate_transfer(dev);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+
+	/* First send over the size of the actual message */
+	err = mhu_v2_x_channel_send(dev, chan, (uint32_t)size);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+	chan++;
+
+	p = (uint32_t *)send_buffer;
+	for (i = 0; i < size; i += 4) {
+		err = mhu_v2_x_channel_send(dev, chan, *p++);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+		if (++chan == (num_channels - 1)) {
+			err = signal_and_wait_for_clear();
+			if (err != MHU_V_2_X_ERR_NONE) {
+				return error_mapping_to_mhu_error_t(err);
+			}
+			chan = 0;
+		}
+	}
+
+	/* Signal the end of transfer.
+	 *   It's not required to send a signal when the message was
+	 *   perfectly-aligned (num_channels - 1 channels were used in the last
+	 *   round) preventing it from signaling twice at the end of transfer.
+	 */
+	if (chan != 0) {
+		err = signal_and_wait_for_clear();
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+	}
+
+	err = mhu_v2_x_close_transfer(dev);
+	return error_mapping_to_mhu_error_t(err);
+}
+
+/*
+ * Public function. See mhu.h
+ *
+ * The basic steps of receiving a message:
+ * 1.	Read the size of the payload from Channel 1. It is the very first
+ *	4 Bytes of the transfer. Continue with Channel 2.
+ * 2.	Receive the payload, read the channels one after the other
+ *	(4 Bytes each). The last available channel is reserved for controlling
+ *	the transfer.
+ *	When the last channel is reached clear all the channels
+ *	(also sending an acknowledge on the last channel).
+ * 3.	If there is still data to receive wait for a notification on the last
+ *	channel and jump to step 2 as soon as it arrived. Otherwise, proceed.
+ * 4.	End of transfer.
+ *
+ */
+enum mhu_error_t mhu_receive_data(uint8_t *receive_buffer, size_t *size)
+{
+	enum mhu_v2_x_error_t err;
+	struct mhu_v2_x_dev_t *dev = &MHU1_SEH_DEV;
+	uint32_t num_channels = mhu_v2_x_get_num_channel_implemented(dev);
+	uint32_t chan = 0;
+	uint32_t message_len;
+	uint32_t i;
+	uint32_t *p;
+
+	/* For simplicity, require:
+	 * - the receive_buffer to be 4-byte aligned,
+	 * - the buffer size to be a multiple of 4.
+	 */
+	if (((uintptr_t)receive_buffer & 0x3U) || (*size & 0x3U)) {
+		return MHU_ERR_INVALID_ARG;
+	}
+
+	/* Busy wait for incoming reply */
+	err = wait_for_signal();
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+
+	/* The first word is the length of the actual message */
+	err = mhu_v2_x_channel_receive(dev, chan, &message_len);
+	if (err != MHU_V_2_X_ERR_NONE) {
+		return error_mapping_to_mhu_error_t(err);
+	}
+	chan++;
+
+	if (message_len > *size) {
+		/* Message buffer too small */
+		*size = message_len;
+		return MHU_ERR_BUFFER_TOO_SMALL;
+	}
+
+	p = (uint32_t *)receive_buffer;
+	for (i = 0; i < message_len; i += 4) {
+		err = mhu_v2_x_channel_receive(dev, chan, p++);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+
+		/* Only wait for next transfer if there is still missing data */
+		if (++chan == (num_channels - 1) && (message_len - i) > 4) {
+			/* Busy wait for next transfer */
+			err = clear_and_wait_for_next_signal();
+			if (err != MHU_V_2_X_ERR_NONE) {
+				return error_mapping_to_mhu_error_t(err);
+			}
+			chan = 0;
+		}
+	}
+
+	/* Clear all channels */
+	for (i = 0; i < num_channels; ++i) {
+		err = mhu_v2_x_channel_clear(dev, i);
+		if (err != MHU_V_2_X_ERR_NONE) {
+			return error_mapping_to_mhu_error_t(err);
+		}
+	}
+
+	*size = message_len;
+
+	return MHU_ERR_NONE;
+}
+
+size_t mhu_get_max_message_size(void)
+{
+	struct mhu_v2_x_dev_t *dev = &MHU1_SEH_DEV;
+	uint32_t num_channels = mhu_v2_x_get_num_channel_implemented(dev);
+
+	assert(num_channels != 0);
+
+	return num_channels * sizeof(uint32_t);
+}
diff --git a/drivers/arm/pl011/aarch32/pl011_console.S b/drivers/arm/pl011/aarch32/pl011_console.S
new file mode 100644
index 0000000..9caeb0c
--- /dev/null
+++ b/drivers/arm/pl011/aarch32/pl011_console.S
@@ -0,0 +1,264 @@
+/*
+ * Copyright (c) 2016-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch.h>
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <console_macros.S>
+#include <drivers/arm/pl011.h>
+
+	/*
+	 * "core" functions are low-level implementations that don't require
+	 * writeable memory and are thus safe to call in BL1 crash context.
+	 */
+	.globl	console_pl011_core_init
+	.globl	console_pl011_core_putc
+	.globl	console_pl011_core_getc
+	.globl	console_pl011_core_flush
+
+	.globl	console_pl011_putc
+	.globl	console_pl011_getc
+	.globl	console_pl011_flush
+
+
+	/* -----------------------------------------------
+	 * int console_core_init(uintptr_t base_addr,
+	 * unsigned int uart_clk, unsigned int baud_rate)
+	 * Function to initialize the console without a
+	 * C Runtime to print debug information. This
+	 * function will be accessed by console_init and
+	 * crash reporting.
+	 * In: r0 - console base address
+	 *     r1 - Uart clock in Hz
+	 *     r2 - Baud rate
+	 * Out: return 1 on success else 0 on error
+	 * Clobber list : r1, r2, r3
+	 * -----------------------------------------------
+	 */
+func console_pl011_core_init
+	/* Check the input base address */
+	cmp	r0, #0
+	beq	core_init_fail
+#if !PL011_GENERIC_UART
+	/* Check baud rate and uart clock for sanity */
+	cmp	r1, #0
+	beq	core_init_fail
+	cmp	r2, #0
+	beq	core_init_fail
+	/* Disable the UART before initialization */
+	ldr	r3, [r0, #UARTCR]
+	bic	r3, r3, #PL011_UARTCR_UARTEN
+	str	r3, [r0, #UARTCR]
+	/* Program the baudrate */
+	/* Divisor =  (Uart clock * 4) / baudrate */
+	lsl	r1, r1, #2
+#if (ARM_ARCH_MAJOR == 7) && !defined(ARMV7_SUPPORTS_VIRTUALIZATION)
+	push	{r0,r3}
+	softudiv	r0,r1,r2,r3
+	mov	r2, r0
+	pop	{r0,r3}
+#else
+	udiv	r2, r1, r2
+#endif
+	/* IBRD = Divisor >> 6 */
+	lsr	r1, r2, #6
+	/* Write the IBRD */
+	str	r1, [r0, #UARTIBRD]
+	/* FBRD = Divisor & 0x3F */
+	and	r1, r2, #0x3f
+	/* Write the FBRD */
+	str	r1, [r0, #UARTFBRD]
+	mov	r1, #PL011_LINE_CONTROL
+	str	r1, [r0, #UARTLCR_H]
+	/* Clear any pending errors */
+	mov	r1, #0
+	str	r1, [r0, #UARTECR]
+	/* Enable tx, rx, and uart overall */
+	ldr	r1, =(PL011_UARTCR_RXE | PL011_UARTCR_TXE | PL011_UARTCR_UARTEN)
+	str	r1, [r0, #UARTCR]
+#endif
+	mov	r0, #1
+	bx	lr
+core_init_fail:
+	mov	r0, #0
+	bx	lr
+endfunc console_pl011_core_init
+
+	.globl console_pl011_register
+
+	/* -------------------------------------------------------
+	 * int console_pl011_register(uintptr_t baseaddr,
+	 *     uint32_t clock, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new PL011
+	 * console. Storage passed in for the console struct
+	 * *must* be persistent (i.e. not from the stack).
+	 * In: r0 - UART register base address
+	 *     r1 - UART clock in Hz
+	 *     r2 - Baud rate
+	 *     r3 - pointer to empty console_t struct
+	 * Out: return 1 on success, 0 on error
+	 * Clobber list : r0, r1, r2
+	 * -------------------------------------------------------
+	 */
+func console_pl011_register
+	push	{r4, lr}
+	mov	r4, r3
+	cmp	r4, #0
+	beq	register_fail
+	str	r0, [r4, #CONSOLE_T_BASE]
+
+	bl console_pl011_core_init
+	cmp	r0, #0
+	beq	register_fail
+
+	mov	r0, r4
+	pop	{r4, lr}
+	finish_console_register pl011 putc=1, getc=1, flush=1
+
+register_fail:
+	pop	{r4, pc}
+endfunc console_pl011_register
+
+	/* --------------------------------------------------------
+	 * int console_core_putc(int c, uintptr_t base_addr)
+	 * Function to output a character over the console. It
+	 * returns the character printed on success or -1 on error.
+	 * In : r0 - character to be printed
+	 *      r1 - console base address
+	 * Out : return -1 on error else return character.
+	 * Clobber list : r2
+	 * --------------------------------------------------------
+	 */
+func console_pl011_core_putc
+	/* Check the input parameter */
+	cmp	r1, #0
+	beq	putc_error
+	/* Prepend '\r' to '\n' */
+	cmp	r0, #0xA
+	bne	2f
+1:
+	/* Check if the transmit FIFO is full */
+	ldr	r2, [r1, #UARTFR]
+	tst	r2, #PL011_UARTFR_TXFF
+	bne	1b
+	mov	r2, #0xD
+	str	r2, [r1, #UARTDR]
+2:
+	/* Check if the transmit FIFO is full */
+	ldr	r2, [r1, #UARTFR]
+	tst	r2, #PL011_UARTFR_TXFF
+	bne	2b
+	str	r0, [r1, #UARTDR]
+	bx	lr
+putc_error:
+	mov	r0, #-1
+	bx	lr
+endfunc console_pl011_core_putc
+
+	/* --------------------------------------------------------
+	 * int console_pl011_putc(int c, console_t *console)
+	 * Function to output a character over the console. It
+	 * returns the character printed on success or -1 on error.
+	 * In: r0 - character to be printed
+	 *     r1 - pointer to console_t structure
+	 * Out : return -1 on error else return character.
+	 * Clobber list: r2
+	 * -------------------------------------------------------
+	 */
+func console_pl011_putc
+#if ENABLE_ASSERTIONS
+	cmp	r1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r1, [r1, #CONSOLE_T_BASE]
+	b	console_pl011_core_putc
+endfunc console_pl011_putc
+
+	/* ---------------------------------------------
+	 * int console_core_getc(uintptr_t base_addr)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 on error.
+	 * In : r0 - console base address
+	 * Clobber list : r0, r1
+	 * ---------------------------------------------
+	 */
+func console_pl011_core_getc
+	cmp	r0, #0
+	beq	getc_error
+1:
+	/* Check if the receive FIFO is empty */
+	ldr	r1, [r0, #UARTFR]
+	tst	r1, #PL011_UARTFR_RXFE
+	bne	1b
+	ldr	r1, [r0, #UARTDR]
+	mov	r0, r1
+	bx	lr
+getc_error:
+	mov	r0, #-1
+	bx	lr
+endfunc console_pl011_core_getc
+
+	/* ------------------------------------------------
+	 * int console_pl011_getc(console_t *console)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 if no character is available.
+	 * In : r0 - pointer to console_t structure
+	 * Out: r0 - character if available, else -1
+	 * Clobber list: r0, r1
+	 * ------------------------------------------------
+	 */
+func console_pl011_getc
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r0, [r0, #CONSOLE_T_BASE]
+	b	console_pl011_core_getc
+endfunc console_pl011_getc
+
+	/* ---------------------------------------------
+	 * void console_core_flush(uintptr_t base_addr)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output.
+	 * In : r0 - console base address
+	 * Out : void
+	 * Clobber list : r0, r1
+	 * ---------------------------------------------
+	 */
+func console_pl011_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+1:
+	/* Loop while the transmit FIFO is busy */
+	ldr	r1, [r0, #UARTFR]
+	tst	r1, #PL011_UARTFR_BUSY
+	bne	1b
+
+	bx	lr
+endfunc console_pl011_core_flush
+
+	/* ---------------------------------------------
+	 * void console_pl011_flush(console_t *console)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output.
+	 * In : r0 - pointer to console_t structure
+	 * Out : void
+	 * Clobber list: r0, r1
+	 * ---------------------------------------------
+	 */
+func console_pl011_flush
+#if ENABLE_ASSERTIONS
+	cmp	r0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	r0, [r0, #CONSOLE_T_BASE]
+	b	console_pl011_core_flush
+endfunc console_pl011_flush
diff --git a/drivers/arm/pl011/aarch64/pl011_console.S b/drivers/arm/pl011/aarch64/pl011_console.S
new file mode 100644
index 0000000..861d2ed
--- /dev/null
+++ b/drivers/arm/pl011/aarch64/pl011_console.S
@@ -0,0 +1,247 @@
+/*
+ * Copyright (c) 2013-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+#include <arch.h>
+#include <asm_macros.S>
+#include <assert_macros.S>
+#include <console_macros.S>
+#include <drivers/arm/pl011.h>
+
+	/*
+	 * "core" functions are low-level implementations that don't require
+	 * writable memory and are thus safe to call in BL1 crash context.
+	 */
+	.globl console_pl011_core_init
+	.globl console_pl011_core_putc
+	.globl console_pl011_core_getc
+	.globl console_pl011_core_flush
+
+	.globl	console_pl011_putc
+	.globl	console_pl011_getc
+	.globl	console_pl011_flush
+
+	/* -----------------------------------------------
+	 * int console_pl011_core_init(uintptr_t base_addr,
+	 * unsigned int uart_clk, unsigned int baud_rate)
+	 * Function to initialize the console without a
+	 * C Runtime to print debug information. This
+	 * function will be accessed by console_init and
+	 * crash reporting.
+	 * In: x0 - console base address
+	 *     w1 - Uart clock in Hz
+	 *     w2 - Baud rate
+	 * Out: return 1 on success else 0 on error
+	 * Clobber list : x1, x2, x3, x4
+	 * -----------------------------------------------
+	 */
+func console_pl011_core_init
+	/* Check the input base address */
+	cbz	x0, core_init_fail
+#if !PL011_GENERIC_UART
+	/* Check baud rate and uart clock for sanity */
+	cbz	w1, core_init_fail
+	cbz	w2, core_init_fail
+	/* Disable uart before programming */
+	ldr	w3, [x0, #UARTCR]
+	mov	w4, #PL011_UARTCR_UARTEN
+	bic	w3, w3, w4
+	str	w3, [x0, #UARTCR]
+	/* Program the baudrate */
+	/* Divisor =  (Uart clock * 4) / baudrate */
+	lsl	w1, w1, #2
+	udiv	w2, w1, w2
+	/* IBRD = Divisor >> 6 */
+	lsr	w1, w2, #6
+	/* Write the IBRD */
+	str	w1, [x0, #UARTIBRD]
+	/* FBRD = Divisor & 0x3F */
+	and	w1, w2, #0x3f
+	/* Write the FBRD */
+	str	w1, [x0, #UARTFBRD]
+	mov	w1, #PL011_LINE_CONTROL
+	str	w1, [x0, #UARTLCR_H]
+	/* Clear any pending errors */
+	str	wzr, [x0, #UARTECR]
+	/* Enable tx, rx, and uart overall */
+	mov	w1, #(PL011_UARTCR_RXE | PL011_UARTCR_TXE | PL011_UARTCR_UARTEN)
+	str	w1, [x0, #UARTCR]
+#endif
+	mov	w0, #1
+	ret
+core_init_fail:
+	mov	w0, wzr
+	ret
+endfunc console_pl011_core_init
+
+	.globl console_pl011_register
+
+	/* -----------------------------------------------
+	 * int console_pl011_register(uintptr_t baseaddr,
+	 *     uint32_t clock, uint32_t baud,
+	 *     console_t *console);
+	 * Function to initialize and register a new PL011
+	 * console. Storage passed in for the console struct
+	 * *must* be persistent (i.e. not from the stack).
+	 * In: x0 - UART register base address
+	 *     w1 - UART clock in Hz
+	 *     w2 - Baud rate
+	 *     x3 - pointer to empty console_t struct
+	 * Out: return 1 on success, 0 on error
+	 * Clobber list : x0, x1, x2, x6, x7, x14
+	 * -----------------------------------------------
+	 */
+func console_pl011_register
+	mov	x7, x30
+	mov	x6, x3
+	cbz	x6, register_fail
+	str	x0, [x6, #CONSOLE_T_BASE]
+
+	bl	console_pl011_core_init
+	cbz	x0, register_fail
+
+	mov	x0, x6
+	mov	x30, x7
+	finish_console_register pl011 putc=1, getc=1, flush=1
+
+register_fail:
+	ret	x7
+endfunc console_pl011_register
+
+	/* --------------------------------------------------------
+	 * int console_pl011_core_putc(int c, uintptr_t base_addr)
+	 * Function to output a character over the console. It
+	 * returns the character printed on success or -1 on error.
+	 * In : w0 - character to be printed
+	 *      x1 - console base address
+	 * Out : return -1 on error else return character.
+	 * Clobber list : x2
+	 * --------------------------------------------------------
+	 */
+func console_pl011_core_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Prepend '\r' to '\n' */
+	cmp	w0, #0xA
+	b.ne	2f
+1:
+	/* Check if the transmit FIFO is full */
+	ldr	w2, [x1, #UARTFR]
+	tbnz	w2, #PL011_UARTFR_TXFF_BIT, 1b
+	mov	w2, #0xD
+	str	w2, [x1, #UARTDR]
+2:
+	/* Check if the transmit FIFO is full */
+	ldr	w2, [x1, #UARTFR]
+	tbnz	w2, #PL011_UARTFR_TXFF_BIT, 2b
+	str	w0, [x1, #UARTDR]
+	ret
+endfunc console_pl011_core_putc
+
+	/* --------------------------------------------------------
+	 * int console_pl011_putc(int c, console_t *console)
+	 * Function to output a character over the console. It
+	 * returns the character printed on success or -1 on error.
+	 * In : w0 - character to be printed
+	 *      x1 - pointer to console_t structure
+	 * Out : return -1 on error else return character.
+	 * Clobber list : x2
+	 * --------------------------------------------------------
+	 */
+func console_pl011_putc
+#if ENABLE_ASSERTIONS
+	cmp	x1, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x1, [x1, #CONSOLE_T_BASE]
+	b	console_pl011_core_putc
+endfunc console_pl011_putc
+
+	/* ---------------------------------------------
+	 * int console_pl011_core_getc(uintptr_t base_addr)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 if no character is available.
+	 * In : x0 - console base address
+	 * Out: w0 - character if available, else -1
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_pl011_core_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+
+	/* Check if the receive FIFO is empty */
+	ldr	w1, [x0, #UARTFR]
+	tbnz	w1, #PL011_UARTFR_RXFE_BIT, no_char
+	ldr	w1, [x0, #UARTDR]
+	mov	w0, w1
+	ret
+no_char:
+	mov	w0, #ERROR_NO_PENDING_CHAR
+	ret
+endfunc console_pl011_core_getc
+
+	/* ---------------------------------------------
+	 * int console_pl011_getc(console_t *console)
+	 * Function to get a character from the console.
+	 * It returns the character grabbed on success
+	 * or -1 if no character is available.
+	 * In : x0 - pointer to console_t structure
+	 * Out: w0 - character if available, else -1
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_pl011_getc
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_pl011_core_getc
+endfunc console_pl011_getc
+
+	/* ---------------------------------------------
+	 * void console_pl011_core_flush(uintptr_t base_addr)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output.
+	 * In : x0 - console base address
+	 * Out : void.
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_pl011_core_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+1:
+	/* Loop until the transmit FIFO is empty */
+	ldr	w1, [x0, #UARTFR]
+	tbnz	w1, #PL011_UARTFR_BUSY_BIT, 1b
+	ret
+endfunc console_pl011_core_flush
+
+	/* ---------------------------------------------
+	 * void console_pl011_flush(console_t *console)
+	 * Function to force a write of all buffered
+	 * data that hasn't been output.
+	 * In : x0 - pointer to console_t structure
+	 * Out : void
+	 * Clobber list : x0, x1
+	 * ---------------------------------------------
+	 */
+func console_pl011_flush
+#if ENABLE_ASSERTIONS
+	cmp	x0, #0
+	ASM_ASSERT(ne)
+#endif /* ENABLE_ASSERTIONS */
+	ldr	x0, [x0, #CONSOLE_T_BASE]
+	b	console_pl011_core_flush
+endfunc console_pl011_flush
diff --git a/drivers/arm/pl061/pl061_gpio.c b/drivers/arm/pl061/pl061_gpio.c
new file mode 100644
index 0000000..97013e8
--- /dev/null
+++ b/drivers/arm/pl061/pl061_gpio.c
@@ -0,0 +1,142 @@
+/*
+ * Copyright (c) 2016, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * ARM PL061 GPIO Driver.
+ * Reference to ARM DDI 0190B document.
+ *
+ */
+
+#include <assert.h>
+#include <errno.h>
+
+#include <common/debug.h>
+#include <drivers/arm/pl061_gpio.h>
+#include <drivers/gpio.h>
+#include <lib/cassert.h>
+#include <lib/mmio.h>
+#include <lib/utils.h>
+
+#if !PLAT_PL061_MAX_GPIOS
+# define PLAT_PL061_MAX_GPIOS	32
+#endif	/* PLAT_PL061_MAX_GPIOS */
+
+CASSERT(PLAT_PL061_MAX_GPIOS > 0, assert_plat_pl061_max_gpios);
+
+#define MAX_GPIO_DEVICES	((PLAT_PL061_MAX_GPIOS +		\
+				 (GPIOS_PER_PL061 - 1)) / GPIOS_PER_PL061)
+
+#define PL061_GPIO_DIR		0x400
+
+#define GPIOS_PER_PL061		8
+
+static int pl061_get_direction(int gpio);
+static void pl061_set_direction(int gpio, int direction);
+static int pl061_get_value(int gpio);
+static void pl061_set_value(int gpio, int value);
+
+static uintptr_t pl061_reg_base[MAX_GPIO_DEVICES];
+
+static const gpio_ops_t pl061_gpio_ops = {
+	.get_direction	= pl061_get_direction,
+	.set_direction	= pl061_set_direction,
+	.get_value	= pl061_get_value,
+	.set_value	= pl061_set_value,
+};
+
+static int pl061_get_direction(int gpio)
+{
+	uintptr_t base_addr;
+	unsigned int data, offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	data = mmio_read_8(base_addr + PL061_GPIO_DIR);
+	if (data & BIT(offset))
+		return GPIO_DIR_OUT;
+	return GPIO_DIR_IN;
+}
+
+static void pl061_set_direction(int gpio, int direction)
+{
+	uintptr_t base_addr;
+	unsigned int data, offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	if (direction == GPIO_DIR_OUT) {
+		data = mmio_read_8(base_addr + PL061_GPIO_DIR) | BIT(offset);
+		mmio_write_8(base_addr + PL061_GPIO_DIR, data);
+	} else {
+		data = mmio_read_8(base_addr + PL061_GPIO_DIR) & ~BIT(offset);
+		mmio_write_8(base_addr + PL061_GPIO_DIR, data);
+	}
+}
+
+/*
+ * The offset of GPIODATA register is 0.
+ * The values read from GPIODATA are determined for each bit, by the mask bit
+ * derived from the address used to access the data register, PADDR[9:2].
+ * Bits that are 1 in the address mask cause the corresponding bits in GPIODATA
+ * to be read, and bits that are 0 in the address mask cause the corresponding
+ * bits in GPIODATA to be read as 0, regardless of their value.
+ */
+static int pl061_get_value(int gpio)
+{
+	uintptr_t base_addr;
+	unsigned int offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	if (mmio_read_8(base_addr + BIT(offset + 2)))
+		return GPIO_LEVEL_HIGH;
+	return GPIO_LEVEL_LOW;
+}
+
+/*
+ * In order to write GPIODATA, the corresponding bits in the mask, resulting
+ * from the address bus, PADDR[9:2], must be HIGH. Otherwise the bit values
+ * remain unchanged by the write.
+ */
+static void pl061_set_value(int gpio, int value)
+{
+	uintptr_t base_addr;
+	int offset;
+
+	assert((gpio >= 0) && (gpio < PLAT_PL061_MAX_GPIOS));
+
+	base_addr = pl061_reg_base[gpio / GPIOS_PER_PL061];
+	offset = gpio % GPIOS_PER_PL061;
+	if (value == GPIO_LEVEL_HIGH)
+		mmio_write_8(base_addr + BIT(offset + 2), BIT(offset));
+	else
+		mmio_write_8(base_addr + BIT(offset + 2), 0);
+}
+
+
+/*
+ * Register the PL061 GPIO controller with a base address and the offset
+ * of start pin in this GPIO controller.
+ * This function is called after pl061_gpio_ops_init().
+ */
+void pl061_gpio_register(uintptr_t base_addr, int gpio_dev)
+{
+	assert((gpio_dev >= 0) && (gpio_dev < MAX_GPIO_DEVICES));
+
+	pl061_reg_base[gpio_dev] = base_addr;
+}
+
+/*
+ * Initialize PL061 GPIO controller with the total GPIO numbers in SoC.
+ */
+void pl061_gpio_init(void)
+{
+	gpio_init(&pl061_gpio_ops);
+}
diff --git a/drivers/arm/rss/rss_comms.c b/drivers/arm/rss/rss_comms.c
new file mode 100644
index 0000000..5e224e1
--- /dev/null
+++ b/drivers/arm/rss/rss_comms.c
@@ -0,0 +1,170 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include <drivers/arm/mhu.h>
+#include <drivers/arm/rss_comms.h>
+#include <psa/client.h>
+#include <rss_comms_protocol.h>
+
+/* Union as message space and reply space are never used at the same time, and this saves space as
+ * we can overlap them.
+ */
+union __packed __attribute__((aligned(4))) rss_comms_io_buffer_t {
+	struct serialized_rss_comms_msg_t msg;
+	struct serialized_rss_comms_reply_t reply;
+};
+
+static uint8_t select_protocol_version(const psa_invec *in_vec, size_t in_len,
+				       const psa_outvec *out_vec, size_t out_len)
+{
+	size_t comms_mhu_msg_size;
+	size_t comms_embed_msg_min_size;
+	size_t comms_embed_reply_min_size;
+	size_t in_size_total = 0;
+	size_t out_size_total = 0;
+	size_t i;
+
+	for (i = 0U; i < in_len; ++i) {
+		in_size_total += in_vec[i].len;
+	}
+	for (i = 0U; i < out_len; ++i) {
+		out_size_total += out_vec[i].len;
+	}
+
+	comms_mhu_msg_size = mhu_get_max_message_size();
+
+	comms_embed_msg_min_size = sizeof(struct serialized_rss_comms_header_t) +
+				   sizeof(struct rss_embed_msg_t) -
+				   PLAT_RSS_COMMS_PAYLOAD_MAX_SIZE;
+
+	comms_embed_reply_min_size = sizeof(struct serialized_rss_comms_header_t) +
+				     sizeof(struct rss_embed_reply_t) -
+				     PLAT_RSS_COMMS_PAYLOAD_MAX_SIZE;
+
+	/* Use embed if we can pack into one message and reply, else use
+	 * pointer_access. The underlying MHU transport protocol uses a
+	 * single uint32_t to track the length, so the amount of data that
+	 * can be in a message is 4 bytes less than mhu_get_max_message_size
+	 * reports.
+	 *
+	 * TODO tune this with real performance numbers, it's possible a
+	 * pointer_access message is less performant than multiple embed
+	 * messages due to ATU configuration costs to allow access to the
+	 * pointers.
+	 */
+	if ((comms_embed_msg_min_size + in_size_total > comms_mhu_msg_size - sizeof(uint32_t))
+	 || (comms_embed_reply_min_size + out_size_total > comms_mhu_msg_size) - sizeof(uint32_t)) {
+		return RSS_COMMS_PROTOCOL_POINTER_ACCESS;
+	} else {
+		return RSS_COMMS_PROTOCOL_EMBED;
+	}
+}
+
+psa_status_t psa_call(psa_handle_t handle, int32_t type, const psa_invec *in_vec, size_t in_len,
+		      psa_outvec *out_vec, size_t out_len)
+{
+	/* Declared statically to avoid using huge amounts of stack space. Maybe revisit if
+	 * functions not being reentrant becomes a problem.
+	 */
+	static union rss_comms_io_buffer_t io_buf;
+	enum mhu_error_t err;
+	psa_status_t status;
+	static uint8_t seq_num = 1U;
+	size_t msg_size;
+	size_t reply_size = sizeof(io_buf.reply);
+	psa_status_t return_val;
+	size_t idx;
+
+	if (type > INT16_MAX || type < INT16_MIN || in_len > PSA_MAX_IOVEC
+	    || out_len > PSA_MAX_IOVEC) {
+		return PSA_ERROR_INVALID_ARGUMENT;
+	}
+
+	io_buf.msg.header.seq_num = seq_num,
+	/* No need to distinguish callers (currently concurrent calls are not supported). */
+	io_buf.msg.header.client_id = 1U,
+	io_buf.msg.header.protocol_ver = select_protocol_version(in_vec, in_len, out_vec, out_len);
+
+	status = rss_protocol_serialize_msg(handle, type, in_vec, in_len, out_vec,
+					    out_len, &io_buf.msg, &msg_size);
+	if (status != PSA_SUCCESS) {
+		return status;
+	}
+
+	VERBOSE("[RSS-COMMS] Sending message\n");
+	VERBOSE("protocol_ver=%u\n", io_buf.msg.header.protocol_ver);
+	VERBOSE("seq_num=%u\n", io_buf.msg.header.seq_num);
+	VERBOSE("client_id=%u\n", io_buf.msg.header.client_id);
+	for (idx = 0; idx < in_len; idx++) {
+		VERBOSE("in_vec[%lu].len=%lu\n", idx, in_vec[idx].len);
+		VERBOSE("in_vec[%lu].buf=%p\n", idx, (void *)in_vec[idx].base);
+	}
+
+	err = mhu_send_data((uint8_t *)&io_buf.msg, msg_size);
+	if (err != MHU_ERR_NONE) {
+		return PSA_ERROR_COMMUNICATION_FAILURE;
+	}
+
+#if DEBUG
+	/*
+	 * Poisoning the message buffer (with a known pattern).
+	 * Helps in detecting hypothetical RSS communication bugs.
+	 */
+	memset(&io_buf.msg, 0xA5, msg_size);
+#endif
+
+	err = mhu_receive_data((uint8_t *)&io_buf.reply, &reply_size);
+	if (err != MHU_ERR_NONE) {
+		return PSA_ERROR_COMMUNICATION_FAILURE;
+	}
+
+	VERBOSE("[RSS-COMMS] Received reply\n");
+	VERBOSE("protocol_ver=%u\n", io_buf.reply.header.protocol_ver);
+	VERBOSE("seq_num=%u\n", io_buf.reply.header.seq_num);
+	VERBOSE("client_id=%u\n", io_buf.reply.header.client_id);
+
+	status = rss_protocol_deserialize_reply(out_vec, out_len, &return_val,
+						&io_buf.reply, reply_size);
+	if (status != PSA_SUCCESS) {
+		return status;
+	}
+
+	VERBOSE("return_val=%d\n", return_val);
+	for (idx = 0U; idx < out_len; idx++) {
+		VERBOSE("out_vec[%lu].len=%lu\n", idx, out_vec[idx].len);
+		VERBOSE("out_vec[%lu].buf=%p\n", idx, (void *)out_vec[idx].base);
+	}
+
+	/* Clear the MHU message buffer to remove assets from memory */
+	memset(&io_buf, 0x0, sizeof(io_buf));
+
+	seq_num++;
+
+	return return_val;
+}
+
+int rss_comms_init(uintptr_t mhu_sender_base, uintptr_t mhu_receiver_base)
+{
+	enum mhu_error_t err;
+
+	err = mhu_init_sender(mhu_sender_base);
+	if (err != MHU_ERR_NONE) {
+		ERROR("[RSS-COMMS] Host to RSS MHU driver initialization failed: %d\n", err);
+		return -1;
+	}
+
+	err = mhu_init_receiver(mhu_receiver_base);
+	if (err != MHU_ERR_NONE) {
+		ERROR("[RSS-COMMS] RSS to Host MHU driver initialization failed: %d\n", err);
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/drivers/arm/rss/rss_comms.mk b/drivers/arm/rss/rss_comms.mk
new file mode 100644
index 0000000..c1c994b
--- /dev/null
+++ b/drivers/arm/rss/rss_comms.mk
@@ -0,0 +1,22 @@
+#
+# Copyright (c) 2022, Arm Limited. All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+#
+
+$(warning "RSS driver is an experimental feature")
+
+RSS_COMMS_SOURCES	:=	$(addprefix drivers/arm/rss/,			\
+					rss_comms.c				\
+					rss_comms_protocol.c			\
+					rss_comms_protocol_embed.c		\
+					rss_comms_protocol_pointer_access.c	\
+				)
+
+RSS_COMMS_SOURCES	+=	$(addprefix drivers/arm/mhu/,			\
+					mhu_v2_x.c				\
+					mhu_wrapper_v2_x.c			\
+				)
+
+PLAT_INCLUDES		+=	-Idrivers/arm/rss		\
+				-Idrivers/arm/mhu
diff --git a/drivers/arm/rss/rss_comms_protocol.c b/drivers/arm/rss/rss_comms_protocol.c
new file mode 100644
index 0000000..a1b1b58
--- /dev/null
+++ b/drivers/arm/rss/rss_comms_protocol.c
@@ -0,0 +1,75 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+#include <assert.h>
+
+#include <common/debug.h>
+#include "rss_comms_protocol.h"
+
+psa_status_t rss_protocol_serialize_msg(psa_handle_t handle,
+					int16_t type,
+					const psa_invec *in_vec,
+					uint8_t in_len,
+					const psa_outvec *out_vec,
+					uint8_t out_len,
+					struct serialized_rss_comms_msg_t *msg,
+					size_t *msg_len)
+{
+	psa_status_t status;
+
+	assert(msg != NULL);
+	assert(msg_len != NULL);
+	assert(in_vec != NULL);
+
+	switch (msg->header.protocol_ver) {
+	case RSS_COMMS_PROTOCOL_EMBED:
+		status = rss_protocol_embed_serialize_msg(handle, type, in_vec, in_len, out_vec,
+							  out_len, &msg->msg.embed, msg_len);
+		if (status != PSA_SUCCESS) {
+			return status;
+		}
+		break;
+	case RSS_COMMS_PROTOCOL_POINTER_ACCESS:
+		status = rss_protocol_pointer_access_serialize_msg(handle, type, in_vec, in_len,
+								   out_vec, out_len,
+								   &msg->msg.pointer_access,
+								   msg_len);
+		if (status != PSA_SUCCESS) {
+			return status;
+		}
+		break;
+	default:
+		return PSA_ERROR_NOT_SUPPORTED;
+	}
+
+	*msg_len += sizeof(struct serialized_rss_comms_header_t);
+
+	return PSA_SUCCESS;
+}
+
+psa_status_t rss_protocol_deserialize_reply(psa_outvec *out_vec,
+					    uint8_t out_len,
+					    psa_status_t *return_val,
+					    const struct serialized_rss_comms_reply_t *reply,
+					    size_t reply_size)
+{
+	assert(reply != NULL);
+	assert(return_val != NULL);
+
+	switch (reply->header.protocol_ver) {
+	case RSS_COMMS_PROTOCOL_EMBED:
+		return rss_protocol_embed_deserialize_reply(out_vec, out_len, return_val,
+							    &reply->reply.embed, reply_size);
+	case RSS_COMMS_PROTOCOL_POINTER_ACCESS:
+		return rss_protocol_pointer_access_deserialize_reply(out_vec, out_len, return_val,
+								     &reply->reply.pointer_access,
+								     reply_size);
+	default:
+		return PSA_ERROR_NOT_SUPPORTED;
+	}
+
+	return PSA_SUCCESS;
+}
diff --git a/drivers/arm/rss/rss_comms_protocol.h b/drivers/arm/rss/rss_comms_protocol.h
new file mode 100644
index 0000000..9a38057
--- /dev/null
+++ b/drivers/arm/rss/rss_comms_protocol.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef __RSS_COMMS_PROTOCOL_H__
+#define __RSS_COMMS_PROTOCOL_H__
+
+#include <cdefs.h>
+#include <stdint.h>
+
+#include <psa/client.h>
+#include "rss_comms_protocol_embed.h"
+#include "rss_comms_protocol_pointer_access.h"
+
+enum rss_comms_protocol_version_t {
+	RSS_COMMS_PROTOCOL_EMBED = 0,
+	RSS_COMMS_PROTOCOL_POINTER_ACCESS = 1,
+};
+
+struct __packed serialized_rss_comms_header_t {
+	uint8_t protocol_ver;
+	uint8_t seq_num;
+	uint16_t client_id;
+};
+
+/* MHU message passed from Host to RSS to deliver a PSA client call */
+struct __packed serialized_rss_comms_msg_t {
+	struct serialized_rss_comms_header_t header;
+	union __packed {
+		struct rss_embed_msg_t embed;
+		struct rss_pointer_access_msg_t pointer_access;
+	} msg;
+};
+
+/* MHU reply message to hold the PSA client reply result returned by RSS */
+struct __packed serialized_rss_comms_reply_t {
+	struct serialized_rss_comms_header_t header;
+	union __packed {
+		struct rss_embed_reply_t embed;
+		struct rss_pointer_access_reply_t pointer_access;
+	} reply;
+};
+
+/* in_len and out_len are uint8_ts, therefore if there are more than 255 iovecs
+ * an error may occur.
+ */
+CASSERT(PSA_MAX_IOVEC <= UINT8_MAX, assert_rss_comms_max_iovec_too_large);
+
+psa_status_t rss_protocol_serialize_msg(psa_handle_t handle,
+					int16_t type,
+					const psa_invec *in_vec,
+					uint8_t in_len,
+					const psa_outvec *out_vec,
+					uint8_t out_len,
+					struct serialized_rss_comms_msg_t *msg,
+					size_t *msg_len);
+
+psa_status_t rss_protocol_deserialize_reply(psa_outvec *out_vec,
+					    uint8_t out_len,
+					    psa_status_t *return_val,
+					    const struct serialized_rss_comms_reply_t *reply,
+					    size_t reply_size);
+
+#endif /* __RSS_COMMS_PROTOCOL_H__ */
diff --git a/drivers/arm/rss/rss_comms_protocol_embed.c b/drivers/arm/rss/rss_comms_protocol_embed.c
new file mode 100644
index 0000000..801b7cc
--- /dev/null
+++ b/drivers/arm/rss/rss_comms_protocol_embed.c
@@ -0,0 +1,91 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#include <assert.h>
+#include <string.h>
+
+#include <common/debug.h>
+#include "rss_comms_protocol_embed.h"
+
+#define TYPE_OFFSET	(16U)
+#define TYPE_MASK	(0xFFFFUL << TYPE_OFFSET)
+#define IN_LEN_OFFSET	(8U)
+#define IN_LEN_MASK	(0xFFUL << IN_LEN_OFFSET)
+#define OUT_LEN_OFFSET	(0U)
+#define OUT_LEN_MASK	(0xFFUL << OUT_LEN_OFFSET)
+
+#define PARAM_PACK(type, in_len, out_len)			  \
+	(((((uint32_t)type) << TYPE_OFFSET) & TYPE_MASK)	| \
+	 ((((uint32_t)in_len) << IN_LEN_OFFSET) & IN_LEN_MASK)	| \
+	 ((((uint32_t)out_len) << OUT_LEN_OFFSET) & OUT_LEN_MASK))
+
+psa_status_t rss_protocol_embed_serialize_msg(psa_handle_t handle,
+					      int16_t type,
+					      const psa_invec *in_vec,
+					      uint8_t in_len,
+					      const psa_outvec *out_vec,
+					      uint8_t out_len,
+					      struct rss_embed_msg_t *msg,
+					      size_t *msg_len)
+{
+	uint32_t payload_size = 0;
+	uint32_t i;
+
+	assert(msg != NULL);
+	assert(msg_len != NULL);
+	assert(in_vec != NULL);
+
+	msg->ctrl_param = PARAM_PACK(type, in_len, out_len);
+	msg->handle = handle;
+
+	/* Fill msg iovec lengths */
+	for (i = 0U; i < in_len; ++i) {
+		msg->io_size[i] = in_vec[i].len;
+	}
+	for (i = 0U; i < out_len; ++i) {
+		msg->io_size[in_len + i] = out_vec[i].len;
+	}
+
+	for (i = 0U; i < in_len; ++i) {
+		if (in_vec[i].len > sizeof(msg->trailer) - payload_size) {
+			return PSA_ERROR_INVALID_ARGUMENT;
+		}
+		memcpy(msg->trailer + payload_size, in_vec[i].base, in_vec[i].len);
+		payload_size += in_vec[i].len;
+	}
+
+	/* Output the actual size of the message, to optimize sending */
+	*msg_len = sizeof(*msg) - sizeof(msg->trailer) + payload_size;
+
+	return PSA_SUCCESS;
+}
+
+psa_status_t rss_protocol_embed_deserialize_reply(psa_outvec *out_vec,
+						  uint8_t out_len,
+						  psa_status_t *return_val,
+						  const struct rss_embed_reply_t *reply,
+						  size_t reply_size)
+{
+	uint32_t payload_offset = 0;
+	uint32_t i;
+
+	assert(reply != NULL);
+	assert(return_val != NULL);
+
+	for (i = 0U; i < out_len; ++i) {
+		if (sizeof(reply) - sizeof(reply->trailer) + payload_offset > reply_size) {
+			return PSA_ERROR_INVALID_ARGUMENT;
+		}
+
+		memcpy(out_vec[i].base, reply->trailer + payload_offset, out_vec[i].len);
+		payload_offset += out_vec[i].len;
+	}
+
+	*return_val = reply->return_val;
+
+	return PSA_SUCCESS;
+}
diff --git a/drivers/arm/rss/rss_comms_protocol_embed.h b/drivers/arm/rss/rss_comms_protocol_embed.h
new file mode 100644
index 0000000..c81c795
--- /dev/null
+++ b/drivers/arm/rss/rss_comms_protocol_embed.h
@@ -0,0 +1,47 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef __RSS_COMMS_PROTOCOL_EMBED_H__
+#define __RSS_COMMS_PROTOCOL_EMBED_H__
+
+#include <cdefs.h>
+
+#include <psa/client.h>
+
+#include <platform_def.h>
+
+
+
+struct __packed rss_embed_msg_t {
+	psa_handle_t handle;
+	uint32_t ctrl_param; /* type, in_len, out_len */
+	uint16_t io_size[PSA_MAX_IOVEC];
+	uint8_t trailer[PLAT_RSS_COMMS_PAYLOAD_MAX_SIZE];
+};
+
+struct __packed rss_embed_reply_t {
+	int32_t return_val;
+	uint16_t out_size[PSA_MAX_IOVEC];
+	uint8_t trailer[PLAT_RSS_COMMS_PAYLOAD_MAX_SIZE];
+};
+
+psa_status_t rss_protocol_embed_serialize_msg(psa_handle_t handle,
+					      int16_t type,
+					      const psa_invec *in_vec,
+					      uint8_t in_len,
+					      const psa_outvec *out_vec,
+					      uint8_t out_len,
+					      struct rss_embed_msg_t *msg,
+					      size_t *msg_len);
+
+psa_status_t rss_protocol_embed_deserialize_reply(psa_outvec *out_vec,
+						  uint8_t out_len,
+						  psa_status_t *return_val,
+						  const struct rss_embed_reply_t *reply,
+						  size_t reply_size);
+
+#endif /* __RSS_COMMS_PROTOCOL_EMBED_H__ */
diff --git a/drivers/arm/rss/rss_comms_protocol_pointer_access.c b/drivers/arm/rss/rss_comms_protocol_pointer_access.c
new file mode 100644
index 0000000..5007b9d
--- /dev/null
+++ b/drivers/arm/rss/rss_comms_protocol_pointer_access.c
@@ -0,0 +1,74 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+#include <assert.h>
+
+#include "rss_comms_protocol_pointer_access.h"
+
+#define TYPE_OFFSET	(16U)
+#define TYPE_MASK	(0xFFFFUL << TYPE_OFFSET)
+#define IN_LEN_OFFSET	(8U)
+#define IN_LEN_MASK	(0xFFUL << IN_LEN_OFFSET)
+#define OUT_LEN_OFFSET	(0U)
+#define OUT_LEN_MASK	(0xFFUL << OUT_LEN_OFFSET)
+
+#define PARAM_PACK(type, in_len, out_len)			  \
+	(((((uint32_t)type) << TYPE_OFFSET) & TYPE_MASK)	| \
+	 ((((uint32_t)in_len) << IN_LEN_OFFSET) & IN_LEN_MASK)	| \
+	 ((((uint32_t)out_len) << OUT_LEN_OFFSET) & OUT_LEN_MASK))
+
+psa_status_t rss_protocol_pointer_access_serialize_msg(psa_handle_t handle,
+						       int16_t type,
+						       const psa_invec *in_vec,
+						       uint8_t in_len,
+						       const psa_outvec *out_vec,
+						       uint8_t out_len,
+						       struct rss_pointer_access_msg_t *msg,
+						       size_t *msg_len)
+{
+	unsigned int i;
+
+	assert(msg != NULL);
+	assert(msg_len != NULL);
+	assert(in_vec != NULL);
+
+	msg->ctrl_param = PARAM_PACK(type, in_len, out_len);
+	msg->handle = handle;
+
+	/* Fill msg iovec lengths */
+	for (i = 0U; i < in_len; ++i) {
+		msg->io_sizes[i] = in_vec[i].len;
+		msg->host_ptrs[i] = (uint64_t)in_vec[i].base;
+	}
+	for (i = 0U; i < out_len; ++i) {
+		msg->io_sizes[in_len + i] = out_vec[i].len;
+		msg->host_ptrs[in_len + i] = (uint64_t)out_vec[i].base;
+	}
+
+	*msg_len = sizeof(*msg);
+
+	return PSA_SUCCESS;
+}
+
+psa_status_t rss_protocol_pointer_access_deserialize_reply(psa_outvec *out_vec,
+							   uint8_t out_len,
+							   psa_status_t *return_val,
+							   const struct rss_pointer_access_reply_t *reply,
+							   size_t reply_size)
+{
+	unsigned int i;
+
+	assert(reply != NULL);
+	assert(return_val != NULL);
+
+	for (i = 0U; i < out_len; ++i) {
+		out_vec[i].len = reply->out_sizes[i];
+	}
+
+	*return_val = reply->return_val;
+
+	return PSA_SUCCESS;
+}
diff --git a/drivers/arm/rss/rss_comms_protocol_pointer_access.h b/drivers/arm/rss/rss_comms_protocol_pointer_access.h
new file mode 100644
index 0000000..a4d054b
--- /dev/null
+++ b/drivers/arm/rss/rss_comms_protocol_pointer_access.h
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2022, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ */
+
+#ifndef __RSS_COMMS_PROTOCOL_POINTER_ACCESS_H__
+#define __RSS_COMMS_PROTOCOL_POINTER_ACCESS_H__
+
+#include <cdefs.h>
+
+#include <psa/client.h>
+
+struct __packed rss_pointer_access_msg_t {
+	psa_handle_t handle;
+	uint32_t ctrl_param;
+	uint32_t io_sizes[PSA_MAX_IOVEC];
+	uint64_t host_ptrs[PSA_MAX_IOVEC];
+};
+
+struct __packed rss_pointer_access_reply_t {
+	int32_t return_val;
+	uint32_t out_sizes[PSA_MAX_IOVEC];
+};
+
+psa_status_t rss_protocol_pointer_access_serialize_msg(psa_handle_t handle,
+						       int16_t type,
+						       const psa_invec *in_vec,
+						       uint8_t in_len,
+						       const psa_outvec *out_vec,
+						       uint8_t out_len,
+						       struct rss_pointer_access_msg_t *msg,
+						       size_t *msg_len);
+
+psa_status_t rss_protocol_pointer_access_deserialize_reply(psa_outvec *out_vec,
+							   uint8_t out_len,
+							   psa_status_t *return_val,
+							   const struct rss_pointer_access_reply_t *reply,
+							   size_t reply_size);
+
+#endif /* __RSS_COMMS_PROTOCOL_POINTER_ACCESS_H__ */
diff --git a/drivers/arm/sbsa/sbsa.c b/drivers/arm/sbsa/sbsa.c
new file mode 100644
index 0000000..79c6f26
--- /dev/null
+++ b/drivers/arm/sbsa/sbsa.c
@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2019, ARM Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stdint.h>
+#include <drivers/arm/sbsa.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+
+void sbsa_watchdog_offset_reg_write(uintptr_t base, uint64_t value)
+{
+	assert((value >> SBSA_WDOG_WOR_WIDTH) == 0);
+	mmio_write_32(base + SBSA_WDOG_WOR_LOW_OFFSET,
+		 ((uint32_t)value & UINT32_MAX));
+	mmio_write_32(base + SBSA_WDOG_WOR_HIGH_OFFSET, (uint32_t)(value >> 32));
+}
+
+/*
+ * Start the watchdog timer at base address "base" for a
+ * period of "ms" milliseconds.The watchdog has to be
+ * refreshed within this time period.
+ */
+void sbsa_wdog_start(uintptr_t base, uint64_t ms)
+{
+	uint64_t counter_freq;
+	uint64_t offset_reg_value;
+
+	counter_freq = (uint64_t)plat_get_syscnt_freq2();
+	offset_reg_value = ms * counter_freq / 1000;
+
+	sbsa_watchdog_offset_reg_write(base, offset_reg_value);
+	mmio_write_32(base + SBSA_WDOG_WCS_OFFSET, SBSA_WDOG_WCS_EN);
+}
+
+/* Stop the watchdog */
+void sbsa_wdog_stop(uintptr_t base)
+{
+	mmio_write_32(base + SBSA_WDOG_WCS_OFFSET, (0x0));
+}
diff --git a/drivers/arm/scu/scu.c b/drivers/arm/scu/scu.c
new file mode 100644
index 0000000..aceac92
--- /dev/null
+++ b/drivers/arm/scu/scu.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2019, Arm Limited. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <drivers/arm/scu.h>
+#include <lib/mmio.h>
+#include <plat/common/platform.h>
+#include <stdint.h>
+
+/*******************************************************************************
+ * Turn ON snoop control unit. This is needed to synchronize the data between
+ * CPU's.
+ ******************************************************************************/
+void enable_snoop_ctrl_unit(uintptr_t base)
+{
+	uint32_t scu_ctrl;
+
+	INFO("[SCU]: enabling snoop control unit ... \n");
+
+	assert(base != 0U);
+	scu_ctrl = mmio_read_32(base + SCU_CTRL_REG);
+
+	/* already enabled? */
+	if ((scu_ctrl & SCU_ENABLE_BIT) != 0) {
+		return;
+	}
+
+	scu_ctrl |= SCU_ENABLE_BIT;
+	mmio_write_32(base + SCU_CTRL_REG, scu_ctrl);
+}
+
+/*******************************************************************************
+ * Snoop Control Unit configuration register. This is read-only register and
+ * contains information such as
+ * - number of CPUs present
+ * - is a particular CPU operating in SMP mode or AMP mode
+ * - data cache size of a particular CPU
+ * - does SCU has ACP port
+ * - is L2CPRESENT
+ * NOTE: user of this API should interpert the bits in this register according
+ * to the TRM
+ ******************************************************************************/
+uint32_t read_snoop_ctrl_unit_cfg(uintptr_t base)
+{
+	assert(base != 0U);
+
+	return mmio_read_32(base + SCU_CFG_REG);
+}
diff --git a/drivers/arm/smmu/smmu_v3.c b/drivers/arm/smmu/smmu_v3.c
new file mode 100644
index 0000000..6c6f978
--- /dev/null
+++ b/drivers/arm/smmu/smmu_v3.c
@@ -0,0 +1,182 @@
+/*
+ * Copyright (c) 2017-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <common/debug.h>
+#include <cdefs.h>
+#include <drivers/arm/smmu_v3.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+#include <arch_features.h>
+
+/* SMMU poll number of retries */
+#define SMMU_POLL_TIMEOUT_US	U(1000)
+
+static int smmuv3_poll(uintptr_t smmu_reg, uint32_t mask,
+				uint32_t value)
+{
+	uint32_t reg_val;
+	uint64_t timeout;
+
+	/* Set 1ms timeout value */
+	timeout = timeout_init_us(SMMU_POLL_TIMEOUT_US);
+	do {
+		reg_val = mmio_read_32(smmu_reg);
+		if ((reg_val & mask) == value)
+			return 0;
+	} while (!timeout_elapsed(timeout));
+
+	ERROR("Timeout polling SMMUv3 register @%p\n", (void *)smmu_reg);
+	ERROR("Read value 0x%x, expected 0x%x\n", reg_val,
+		value == 0U ? reg_val & ~mask : reg_val | mask);
+	return -1;
+}
+
+/*
+ * Abort all incoming transactions in order to implement a default
+ * deny policy on reset.
+ */
+int __init smmuv3_security_init(uintptr_t smmu_base)
+{
+	/* Attribute update has completed when SMMU_(S)_GBPA.Update bit is 0 */
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U)
+		return -1;
+
+	/*
+	 * SMMU_(S)_CR0 resets to zero with all streams bypassing the SMMU,
+	 * so just abort all incoming transactions.
+	 */
+	mmio_setbits_32(smmu_base + SMMU_GBPA,
+			SMMU_GBPA_UPDATE | SMMU_GBPA_ABORT);
+
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U)
+		return -1;
+
+	/* Check if the SMMU supports secure state */
+	if ((mmio_read_32(smmu_base + SMMU_S_IDR1) &
+				SMMU_S_IDR1_SECURE_IMPL) == 0U)
+		return 0;
+
+	/* Abort all incoming secure transactions */
+	if (smmuv3_poll(smmu_base + SMMU_S_GBPA, SMMU_S_GBPA_UPDATE, 0U) != 0U)
+		return -1;
+
+	mmio_setbits_32(smmu_base + SMMU_S_GBPA,
+			SMMU_S_GBPA_UPDATE | SMMU_S_GBPA_ABORT);
+
+	return smmuv3_poll(smmu_base + SMMU_S_GBPA, SMMU_S_GBPA_UPDATE, 0U);
+}
+
+/*
+ * Initialize the SMMU by invalidating all secure caches and TLBs.
+ * Abort all incoming transactions in order to implement a default
+ * deny policy on reset
+ */
+int __init smmuv3_init(uintptr_t smmu_base)
+{
+	/* Abort all incoming transactions */
+	if (smmuv3_security_init(smmu_base) != 0)
+		return -1;
+
+#if ENABLE_RME
+
+	if (get_armv9_2_feat_rme_support() != 0U) {
+		if ((mmio_read_32(smmu_base + SMMU_ROOT_IDR0) &
+				  SMMU_ROOT_IDR0_ROOT_IMPL) == 0U) {
+			WARN("Skip SMMU GPC configuration.\n");
+		} else {
+			uint64_t gpccr_el3 = read_gpccr_el3();
+			uint64_t gptbr_el3 = read_gptbr_el3();
+
+			/* SMMU_ROOT_GPT_BASE_CFG[16] is RES0. */
+			gpccr_el3 &= ~(1UL << 16);
+
+			/*
+			 * TODO: SMMU_ROOT_GPT_BASE_CFG is 64b in the spec,
+			 * but SMMU model only accepts 32b access.
+			 */
+			mmio_write_32(smmu_base + SMMU_ROOT_GPT_BASE_CFG,
+				      gpccr_el3);
+
+			/*
+			 * pa_gpt_table_base[51:12] maps to GPTBR_EL3[39:0]
+			 * whereas it maps to SMMU_ROOT_GPT_BASE[51:12]
+			 * hence needs a 12 bit left shit.
+			 */
+			mmio_write_64(smmu_base + SMMU_ROOT_GPT_BASE,
+				      gptbr_el3 << 12);
+
+			/*
+			 * ACCESSEN=1: SMMU- and client-originated accesses are
+			 *             not terminated by this mechanism.
+			 * GPCEN=1: All clients and SMMU-originated accesses,
+			 *          except GPT-walks, are subject to GPC.
+			 */
+			mmio_setbits_32(smmu_base + SMMU_ROOT_CR0,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN);
+
+			/* Poll for ACCESSEN and GPCEN ack bits. */
+			if (smmuv3_poll(smmu_base + SMMU_ROOT_CR0ACK,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN,
+					SMMU_ROOT_CR0_GPCEN |
+					SMMU_ROOT_CR0_ACCESSEN) != 0) {
+				WARN("Failed enabling SMMU GPC.\n");
+
+				/*
+				 * Do not return in error, but fall back to
+				 * invalidating all entries through the secure
+				 * register file.
+				 */
+			}
+		}
+	}
+
+#endif /* ENABLE_RME */
+
+	/*
+	 * Initiate invalidation of secure caches and TLBs if the SMMU
+	 * supports secure state. If not, it's implementation defined
+	 * as to how SMMU_S_INIT register is accessed.
+	 * Arm SMMU Arch RME supplement, section 3.4: all SMMU registers
+	 * specified to be accessible only in secure physical address space are
+	 * additionally accessible in root physical address space in an SMMU
+	 * with RME.
+	 * Section 3.3: as GPT information is permitted to be cached in a TLB,
+	 * the SMMU_S_INIT.INV_ALL mechanism also invalidates GPT information
+	 * cached in TLBs.
+	 */
+	mmio_write_32(smmu_base + SMMU_S_INIT, SMMU_S_INIT_INV_ALL);
+
+	/* Wait for global invalidation operation to finish */
+	return smmuv3_poll(smmu_base + SMMU_S_INIT,
+				SMMU_S_INIT_INV_ALL, 0U);
+}
+
+int smmuv3_ns_set_abort_all(uintptr_t smmu_base)
+{
+	/* Attribute update has completed when SMMU_GBPA.Update bit is 0 */
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U) {
+		return -1;
+	}
+
+	/*
+	 * Set GBPA's ABORT bit. Other GBPA fields are presumably ignored then,
+	 * so simply preserve their value.
+	 */
+	mmio_setbits_32(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE | SMMU_GBPA_ABORT);
+	if (smmuv3_poll(smmu_base + SMMU_GBPA, SMMU_GBPA_UPDATE, 0U) != 0U) {
+		return -1;
+	}
+
+	/* Disable the SMMU to engage the GBPA fields previously configured. */
+	mmio_clrbits_32(smmu_base + SMMU_CR0, SMMU_CR0_SMMUEN);
+	if (smmuv3_poll(smmu_base + SMMU_CR0ACK, SMMU_CR0_SMMUEN, 0U) != 0U) {
+		return -1;
+	}
+
+	return 0;
+}
diff --git a/drivers/arm/sp804/sp804_delay_timer.c b/drivers/arm/sp804/sp804_delay_timer.c
new file mode 100644
index 0000000..9c5e762
--- /dev/null
+++ b/drivers/arm/sp804/sp804_delay_timer.c
@@ -0,0 +1,57 @@
+/*
+ * Copyright (c) 2015-2019, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <drivers/arm/sp804_delay_timer.h>
+#include <drivers/delay_timer.h>
+#include <lib/mmio.h>
+
+uintptr_t sp804_base_addr;
+
+#define SP804_TIMER1_LOAD	(sp804_base_addr + 0x000)
+#define SP804_TIMER1_VALUE	(sp804_base_addr + 0x004)
+#define SP804_TIMER1_CONTROL	(sp804_base_addr + 0x008)
+#define SP804_TIMER1_BGLOAD	(sp804_base_addr + 0x018)
+
+#define TIMER_CTRL_ONESHOT	(1 << 0)
+#define TIMER_CTRL_32BIT	(1 << 1)
+#define TIMER_CTRL_DIV1		(0 << 2)
+#define TIMER_CTRL_DIV16	(1 << 2)
+#define TIMER_CTRL_DIV256	(2 << 2)
+#define TIMER_CTRL_IE		(1 << 5)
+#define TIMER_CTRL_PERIODIC	(1 << 6)
+#define TIMER_CTRL_ENABLE	(1 << 7)
+
+/********************************************************************
+ * The SP804 timer delay function
+ ********************************************************************/
+uint32_t sp804_get_timer_value(void)
+{
+	return mmio_read_32(SP804_TIMER1_VALUE);
+}
+
+/********************************************************************
+ * Initialize the 1st timer in the SP804 dual timer with a base
+ * address and a timer ops
+ ********************************************************************/
+void sp804_timer_ops_init(uintptr_t base_addr, const timer_ops_t *ops)
+{
+	assert(base_addr != 0);
+	assert(ops != 0 && ops->get_timer_value == sp804_get_timer_value);
+
+	sp804_base_addr = base_addr;
+	timer_init(ops);
+
+	/* disable timer1 */
+	mmio_write_32(SP804_TIMER1_CONTROL, 0);
+	mmio_write_32(SP804_TIMER1_LOAD, UINT32_MAX);
+	mmio_write_32(SP804_TIMER1_VALUE, UINT32_MAX);
+
+	/* enable as a free running 32-bit counter */
+	mmio_write_32(SP804_TIMER1_CONTROL,
+			TIMER_CTRL_32BIT | TIMER_CTRL_ENABLE);
+}
diff --git a/drivers/arm/sp805/sp805.c b/drivers/arm/sp805/sp805.c
new file mode 100644
index 0000000..ffca1ce
--- /dev/null
+++ b/drivers/arm/sp805/sp805.c
@@ -0,0 +1,51 @@
+/*
+ * Copyright (c) 2015-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <stdint.h>
+
+#include <drivers/arm/sp805.h>
+#include <lib/mmio.h>
+
+/* Inline register access functions */
+
+static inline void sp805_write_wdog_load(uintptr_t base, uint32_t value)
+{
+	mmio_write_32(base + SP805_WDOG_LOAD_OFF, value);
+}
+
+static inline void sp805_write_wdog_ctrl(uintptr_t base, uint32_t value)
+{
+	mmio_write_32(base + SP805_WDOG_CTR_OFF, value);
+}
+
+static inline void sp805_write_wdog_lock(uintptr_t base, uint32_t value)
+{
+	mmio_write_32(base + SP805_WDOG_LOCK_OFF, value);
+}
+
+
+/* Public API implementation */
+
+void sp805_start(uintptr_t base, unsigned int ticks)
+{
+	sp805_write_wdog_load(base, ticks);
+	sp805_write_wdog_ctrl(base, SP805_CTR_RESEN | SP805_CTR_INTEN);
+	/* Lock registers access */
+	sp805_write_wdog_lock(base, 0U);
+}
+
+void sp805_stop(uintptr_t base)
+{
+	sp805_write_wdog_lock(base, WDOG_UNLOCK_KEY);
+	sp805_write_wdog_ctrl(base, 0U);
+}
+
+void sp805_refresh(uintptr_t base, unsigned int ticks)
+{
+	sp805_write_wdog_lock(base, WDOG_UNLOCK_KEY);
+	sp805_write_wdog_load(base, ticks);
+	sp805_write_wdog_lock(base, 0U);
+}
diff --git a/drivers/arm/tzc/tzc380.c b/drivers/arm/tzc/tzc380.c
new file mode 100644
index 0000000..9518748
--- /dev/null
+++ b/drivers/arm/tzc/tzc380.c
@@ -0,0 +1,104 @@
+/*
+ * Copyright (c) 2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+#include <common/debug.h>
+#include <drivers/arm/tzc380.h>
+#include <lib/mmio.h>
+
+struct tzc380_instance {
+	uintptr_t base;
+	uint8_t addr_width;
+	uint8_t num_regions;
+};
+
+struct tzc380_instance tzc380;
+
+static unsigned int tzc380_read_build_config(uintptr_t base)
+{
+	return mmio_read_32(base + TZC380_CONFIGURATION_OFF);
+}
+
+static void tzc380_write_action(uintptr_t base, unsigned int action)
+{
+	mmio_write_32(base + ACTION_OFF, action);
+}
+
+static void tzc380_write_region_base_low(uintptr_t base, unsigned int region,
+				      unsigned int val)
+{
+	mmio_write_32(base + REGION_SETUP_LOW_OFF(region), val);
+}
+
+static void tzc380_write_region_base_high(uintptr_t base, unsigned int region,
+				       unsigned int val)
+{
+	mmio_write_32(base + REGION_SETUP_HIGH_OFF(region), val);
+}
+
+static void tzc380_write_region_attributes(uintptr_t base, unsigned int region,
+					unsigned int val)
+{
+	mmio_write_32(base + REGION_ATTRIBUTES_OFF(region), val);
+}
+
+void tzc380_init(uintptr_t base)
+{
+	unsigned int tzc_build;
+
+	assert(base != 0U);
+	tzc380.base = base;
+
+	/* Save values we will use later. */
+	tzc_build = tzc380_read_build_config(tzc380.base);
+	tzc380.addr_width  = ((tzc_build >> BUILD_CONFIG_AW_SHIFT) &
+			      BUILD_CONFIG_AW_MASK) + 1;
+	tzc380.num_regions = ((tzc_build >> BUILD_CONFIG_NR_SHIFT) &
+			       BUILD_CONFIG_NR_MASK) + 1;
+}
+
+static uint32_t addr_low(uintptr_t addr)
+{
+	return (uint32_t)addr;
+}
+
+static uint32_t addr_high(uintptr_t addr __unused)
+{
+#if (UINTPTR_MAX == UINT64_MAX)
+	return addr >> 32;
+#else
+	return 0;
+#endif
+}
+
+/*
+ * `tzc380_configure_region` is used to program regions into the TrustZone
+ * controller.
+ */
+void tzc380_configure_region(uint8_t region, uintptr_t region_base, unsigned int attr)
+{
+	assert(tzc380.base != 0U);
+
+	assert(region < tzc380.num_regions);
+
+	tzc380_write_region_base_low(tzc380.base, region, addr_low(region_base));
+	tzc380_write_region_base_high(tzc380.base, region, addr_high(region_base));
+	tzc380_write_region_attributes(tzc380.base, region, attr);
+}
+
+void tzc380_set_action(unsigned int action)
+{
+	assert(tzc380.base != 0U);
+
+	/*
+	 * - Currently no handler is provided to trap an error via interrupt
+	 *   or exception.
+	 * - The interrupt action has not been tested.
+	 */
+	tzc380_write_action(tzc380.base, action);
+}
diff --git a/drivers/arm/tzc/tzc400.c b/drivers/arm/tzc/tzc400.c
new file mode 100644
index 0000000..759824d
--- /dev/null
+++ b/drivers/arm/tzc/tzc400.c
@@ -0,0 +1,360 @@
+/*
+ * Copyright (c) 2016-2022, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+#include <stddef.h>
+
+#include <common/debug.h>
+#include <drivers/arm/tzc400.h>
+#include <lib/mmio.h>
+#include <lib/utils_def.h>
+
+#include "tzc_common_private.h"
+
+/*
+ * Macros which will be used by common core functions.
+ */
+#define TZC_400_REGION_BASE_LOW_0_OFFSET	U(0x100)
+#define TZC_400_REGION_BASE_HIGH_0_OFFSET	U(0x104)
+#define TZC_400_REGION_TOP_LOW_0_OFFSET		U(0x108)
+#define TZC_400_REGION_TOP_HIGH_0_OFFSET	U(0x10c)
+#define TZC_400_REGION_ATTR_0_OFFSET		U(0x110)
+#define TZC_400_REGION_ID_ACCESS_0_OFFSET	U(0x114)
+
+/*
+ * Implementation defined values used to validate inputs later.
+ * Filters : max of 4 ; 0 to 3
+ * Regions : max of 9 ; 0 to 8
+ * Address width : Values between 32 to 64
+ */
+typedef struct tzc400_instance {
+	uintptr_t base;
+	uint8_t addr_width;
+	uint8_t num_filters;
+	uint8_t num_regions;
+} tzc400_instance_t;
+
+static tzc400_instance_t tzc400;
+
+static inline unsigned int _tzc400_read_build_config(uintptr_t base)
+{
+	return mmio_read_32(base + BUILD_CONFIG_OFF);
+}
+
+static inline unsigned int _tzc400_read_gate_keeper(uintptr_t base)
+{
+	return mmio_read_32(base + GATE_KEEPER_OFF);
+}
+
+static inline void _tzc400_write_gate_keeper(uintptr_t base, unsigned int val)
+{
+	mmio_write_32(base + GATE_KEEPER_OFF, val);
+}
+
+/*
+ * Get the open status information for all filter units.
+ */
+#define get_gate_keeper_os(_base)	((_tzc400_read_gate_keeper(_base) >>  \
+					GATE_KEEPER_OS_SHIFT) &		\
+					GATE_KEEPER_OS_MASK)
+
+
+/* Define common core functions used across different TZC peripherals. */
+DEFINE_TZC_COMMON_WRITE_ACTION(400, 400)
+DEFINE_TZC_COMMON_WRITE_REGION_BASE(400, 400)
+DEFINE_TZC_COMMON_WRITE_REGION_TOP(400, 400)
+DEFINE_TZC_COMMON_WRITE_REGION_ATTRIBUTES(400, 400)
+DEFINE_TZC_COMMON_WRITE_REGION_ID_ACCESS(400, 400)
+DEFINE_TZC_COMMON_UPDATE_FILTERS(400, 400)
+DEFINE_TZC_COMMON_CONFIGURE_REGION0(400)
+DEFINE_TZC_COMMON_CONFIGURE_REGION(400)
+
+static void _tzc400_clear_it(uintptr_t base, uint32_t filter)
+{
+	mmio_write_32(base + INT_CLEAR, BIT_32(filter));
+}
+
+static uint32_t _tzc400_get_int_by_filter(uintptr_t base, uint32_t filter)
+{
+	return mmio_read_32(base + INT_STATUS) & BIT_32(filter);
+}
+
+#if DEBUG
+static unsigned long _tzc400_get_fail_address(uintptr_t base, uint32_t filter)
+{
+	unsigned long fail_address;
+
+	fail_address = mmio_read_32(base + FAIL_ADDRESS_LOW_OFF +
+				    (filter * FILTER_OFFSET));
+#ifdef __aarch64__
+	fail_address += (unsigned long)mmio_read_32(base + FAIL_ADDRESS_HIGH_OFF +
+						    (filter * FILTER_OFFSET)) << 32;
+#endif
+
+	return fail_address;
+}
+
+static uint32_t _tzc400_get_fail_id(uintptr_t base, uint32_t filter)
+{
+	return mmio_read_32(base + FAIL_ID + (filter * FILTER_OFFSET));
+}
+
+static uint32_t _tzc400_get_fail_control(uintptr_t base, uint32_t filter)
+{
+	return mmio_read_32(base + FAIL_CONTROL_OFF + (filter * FILTER_OFFSET));
+}
+
+static void _tzc400_dump_fail_filter(uintptr_t base, uint32_t filter)
+{
+	uint32_t control_fail;
+	uint32_t fail_id;
+	unsigned long address_fail;
+
+	address_fail = _tzc400_get_fail_address(base, filter);
+	ERROR("Illegal access to 0x%lx:\n", address_fail);
+
+	fail_id = _tzc400_get_fail_id(base, filter);
+	ERROR("\tFAIL_ID = 0x%x\n", fail_id);
+
+	control_fail = _tzc400_get_fail_control(base, filter);
+	if (((control_fail & BIT_32(FAIL_CONTROL_NS_SHIFT)) >> FAIL_CONTROL_NS_SHIFT) ==
+	    FAIL_CONTROL_NS_NONSECURE) {
+		ERROR("\tNon-Secure\n");
+	} else {
+		ERROR("\tSecure\n");
+	}
+
+	if (((control_fail & BIT_32(FAIL_CONTROL_PRIV_SHIFT)) >> FAIL_CONTROL_PRIV_SHIFT) ==
+	    FAIL_CONTROL_PRIV_PRIV) {
+		ERROR("\tPrivilege\n");
+	} else {
+		ERROR("\tUnprivilege\n");
+	}
+
+	if (((control_fail & BIT_32(FAIL_CONTROL_DIR_SHIFT)) >> FAIL_CONTROL_DIR_SHIFT) ==
+	    FAIL_CONTROL_DIR_WRITE) {
+		ERROR("\tWrite\n");
+	} else {
+		ERROR("\tRead\n");
+	}
+}
+#endif /* DEBUG */
+
+static unsigned int _tzc400_get_gate_keeper(uintptr_t base,
+				unsigned int filter)
+{
+	unsigned int open_status;
+
+	open_status = get_gate_keeper_os(base);
+
+	return (open_status >> filter) & GATE_KEEPER_FILTER_MASK;
+}
+
+/* This function is not MP safe. */
+static void _tzc400_set_gate_keeper(uintptr_t base,
+				unsigned int filter,
+				int val)
+{
+	unsigned int open_status;
+
+	/* Upper half is current state. Lower half is requested state. */
+	open_status = get_gate_keeper_os(base);
+
+	if (val != 0)
+		open_status |=  (1UL << filter);
+	else
+		open_status &= ~(1UL << filter);
+
+	_tzc400_write_gate_keeper(base, (open_status & GATE_KEEPER_OR_MASK) <<
+			      GATE_KEEPER_OR_SHIFT);
+
+	/* Wait here until we see the change reflected in the TZC status. */
+	while ((get_gate_keeper_os(base)) != open_status)
+		;
+}
+
+void tzc400_set_action(unsigned int action)
+{
+	assert(tzc400.base != 0U);
+	assert(action <= TZC_ACTION_ERR_INT);
+
+	_tzc400_write_action(tzc400.base, action);
+}
+
+void tzc400_init(uintptr_t base)
+{
+#if DEBUG
+	unsigned int tzc400_id;
+#endif
+	unsigned int tzc400_build;
+
+	assert(base != 0U);
+	tzc400.base = base;
+
+#if DEBUG
+	tzc400_id = _tzc_read_peripheral_id(base);
+	if (tzc400_id != TZC_400_PERIPHERAL_ID) {
+		ERROR("TZC-400 : Wrong device ID (0x%x).\n", tzc400_id);
+		panic();
+	}
+#endif
+
+	/* Save values we will use later. */
+	tzc400_build = _tzc400_read_build_config(tzc400.base);
+	tzc400.num_filters = (uint8_t)((tzc400_build >> BUILD_CONFIG_NF_SHIFT) &
+					BUILD_CONFIG_NF_MASK) + 1U;
+	tzc400.addr_width  = (uint8_t)((tzc400_build >> BUILD_CONFIG_AW_SHIFT) &
+					BUILD_CONFIG_AW_MASK) + 1U;
+	tzc400.num_regions = (uint8_t)((tzc400_build >> BUILD_CONFIG_NR_SHIFT) &
+					BUILD_CONFIG_NR_MASK) + 1U;
+}
+
+/*
+ * `tzc400_configure_region0` is used to program region 0 into the TrustZone
+ * controller. Region 0 covers the whole address space that is not mapped
+ * to any other region, and is enabled on all filters; this cannot be
+ * changed. This function only changes the access permissions.
+ */
+void tzc400_configure_region0(unsigned int sec_attr,
+			   unsigned int ns_device_access)
+{
+	assert(tzc400.base != 0U);
+	assert(sec_attr <= TZC_REGION_S_RDWR);
+
+	_tzc400_configure_region0(tzc400.base, sec_attr, ns_device_access);
+}
+
+/*
+ * `tzc400_configure_region` is used to program regions into the TrustZone
+ * controller. A region can be associated with more than one filter. The
+ * associated filters are passed in as a bitmap (bit0 = filter0), except that
+ * the value TZC_400_REGION_ATTR_FILTER_BIT_ALL selects all filters, based on
+ * the value of tzc400.num_filters.
+ * NOTE:
+ * Region 0 is special; it is preferable to use tzc400_configure_region0
+ * for this region (see comment for that function).
+ */
+void tzc400_configure_region(unsigned int filters,
+			  unsigned int region,
+			  unsigned long long region_base,
+			  unsigned long long region_top,
+			  unsigned int sec_attr,
+			  unsigned int nsaid_permissions)
+{
+	assert(tzc400.base != 0U);
+
+	/* Adjust filter mask by real filter number */
+	if (filters == TZC_400_REGION_ATTR_FILTER_BIT_ALL) {
+		filters = (1U << tzc400.num_filters) - 1U;
+	}
+
+	/* Do range checks on filters and regions. */
+	assert(((filters >> tzc400.num_filters) == 0U) &&
+	       (region < tzc400.num_regions));
+
+	/*
+	 * Do address range check based on TZC configuration. A 64bit address is
+	 * the max and expected case.
+	 */
+	assert((region_top <= (UINT64_MAX >> (64U - tzc400.addr_width))) &&
+		(region_base < region_top));
+
+	/* region_base and (region_top + 1) must be 4KB aligned */
+	assert(((region_base | (region_top + 1U)) & (4096U - 1U)) == 0U);
+
+	assert(sec_attr <= TZC_REGION_S_RDWR);
+
+	_tzc400_configure_region(tzc400.base, filters, region, region_base,
+						region_top,
+						sec_attr, nsaid_permissions);
+}
+
+void tzc400_update_filters(unsigned int region, unsigned int filters)
+{
+	/* Do range checks on filters and regions. */
+	assert(((filters >> tzc400.num_filters) == 0U) &&
+	       (region < tzc400.num_regions));
+
+	_tzc400_update_filters(tzc400.base, region, tzc400.num_filters, filters);
+}
+
+void tzc400_enable_filters(void)
+{
+	unsigned int state;
+	unsigned int filter;
+
+	assert(tzc400.base != 0U);
+
+	for (filter = 0U; filter < tzc400.num_filters; filter++) {
+		state = _tzc400_get_gate_keeper(tzc400.base, filter);
+		if (state != 0U) {
+			/* Filter 0 is special and cannot be disabled.
+			 * So here we allow it being already enabled. */
+			if (filter == 0U) {
+				continue;
+			}
+			/*
+			 * The TZC filter is already configured. Changing the
+			 * programmer's view in an active system can cause
+			 * unpredictable behavior therefore panic for now rather
+			 * than try to determine whether this is safe in this
+			 * instance.
+			 *
+			 * See the 'ARM (R) CoreLink TM TZC-400 TrustZone (R)
+			 * Address Space Controller' Technical Reference Manual.
+			 */
+			ERROR("TZC-400 : Filter %u Gatekeeper already enabled.\n",
+			      filter);
+			panic();
+		}
+		_tzc400_set_gate_keeper(tzc400.base, filter, 1);
+	}
+}
+
+void tzc400_disable_filters(void)
+{
+	unsigned int filter;
+	unsigned int state;
+	unsigned int start = 0U;
+
+	assert(tzc400.base != 0U);
+
+	/* Filter 0 is special and cannot be disabled. */
+	state = _tzc400_get_gate_keeper(tzc400.base, 0);
+	if (state != 0U) {
+		start++;
+	}
+	for (filter = start; filter < tzc400.num_filters; filter++)
+		_tzc400_set_gate_keeper(tzc400.base, filter, 0);
+}
+
+int tzc400_it_handler(void)
+{
+	uint32_t filter;
+	uint32_t filter_it_pending = tzc400.num_filters;
+
+	assert(tzc400.base != 0U);
+
+	for (filter = 0U; filter < tzc400.num_filters; filter++) {
+		if (_tzc400_get_int_by_filter(tzc400.base, filter) != 0U) {
+			filter_it_pending = filter;
+			break;
+		}
+	}
+
+	if (filter_it_pending == tzc400.num_filters) {
+		ERROR("TZC-400: No interrupt pending!\n");
+		return -1;
+	}
+
+#if DEBUG
+	_tzc400_dump_fail_filter(tzc400.base, filter_it_pending);
+#endif
+
+	_tzc400_clear_it(tzc400.base, filter_it_pending);
+
+	return 0;
+}
diff --git a/drivers/arm/tzc/tzc_common_private.h b/drivers/arm/tzc/tzc_common_private.h
new file mode 100644
index 0000000..2090944
--- /dev/null
+++ b/drivers/arm/tzc/tzc_common_private.h
@@ -0,0 +1,204 @@
+/*
+ * Copyright (c) 2016-2021, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#ifndef TZC_COMMON_PRIVATE_H
+#define TZC_COMMON_PRIVATE_H
+
+#include <arch.h>
+#include <arch_helpers.h>
+#include <drivers/arm/tzc_common.h>
+#include <lib/mmio.h>
+
+#define DEFINE_TZC_COMMON_WRITE_ACTION(fn_name, macro_name)		\
+	static inline void _tzc##fn_name##_write_action(		\
+					uintptr_t base,			\
+					unsigned int action)		\
+	{								\
+		mmio_write_32(base + TZC_##macro_name##_ACTION_OFF,	\
+			action);					\
+	}
+
+#define DEFINE_TZC_COMMON_WRITE_REGION_BASE(fn_name, macro_name)	\
+	static inline void _tzc##fn_name##_write_region_base(		\
+					uintptr_t base,			\
+					unsigned int region_no,		\
+					unsigned long long region_base)	\
+	{								\
+		mmio_write_32(base +					\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				(u_register_t)region_no) +		\
+			TZC_##macro_name##_REGION_BASE_LOW_0_OFFSET,	\
+			(uint32_t)region_base);				\
+		mmio_write_32(base +					\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				(u_register_t)region_no) +		\
+			TZC_##macro_name##_REGION_BASE_HIGH_0_OFFSET,	\
+			(uint32_t)(region_base >> 32));			\
+	}
+
+#define DEFINE_TZC_COMMON_WRITE_REGION_TOP(fn_name, macro_name)		\
+	static inline void _tzc##fn_name##_write_region_top(		\
+					uintptr_t base,			\
+					unsigned int region_no,		\
+					unsigned long long region_top)	\
+	{								\
+		mmio_write_32(base +					\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				(u_register_t)region_no) +		\
+			TZC_##macro_name##_REGION_TOP_LOW_0_OFFSET,	\
+			(uint32_t)region_top);				\
+		mmio_write_32(base +					\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				(u_register_t)region_no) +		\
+			TZC_##macro_name##_REGION_TOP_HIGH_0_OFFSET,	\
+			(uint32_t)(region_top >> 32));			\
+	}
+
+#define DEFINE_TZC_COMMON_WRITE_REGION_ATTRIBUTES(fn_name, macro_name)	\
+	static inline void _tzc##fn_name##_write_region_attributes(	\
+						uintptr_t base,		\
+						unsigned int region_no,	\
+						unsigned int attr)	\
+	{								\
+		mmio_write_32(base +					\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				(u_register_t)region_no) +		\
+			TZC_##macro_name##_REGION_ATTR_0_OFFSET,	\
+			attr);						\
+	}
+
+#define DEFINE_TZC_COMMON_WRITE_REGION_ID_ACCESS(fn_name, macro_name)	\
+	static inline void _tzc##fn_name##_write_region_id_access(	\
+						uintptr_t base,		\
+						unsigned int region_no,	\
+						unsigned int val)	\
+	{								\
+		mmio_write_32(base +					\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				(u_register_t)region_no) +		\
+			TZC_##macro_name##_REGION_ID_ACCESS_0_OFFSET,	\
+			val);						\
+	}
+
+/*
+ * It is used to modify the filters status for a defined region.
+ */
+#define DEFINE_TZC_COMMON_UPDATE_FILTERS(fn_name, macro_name)		\
+	static inline void _tzc##fn_name##_update_filters(		\
+						uintptr_t base,		\
+						unsigned int region_no,	\
+						unsigned int nbfilters, \
+						unsigned int filters)	\
+	{								\
+		uint32_t filters_mask = GENMASK(nbfilters - 1U, 0);	\
+									\
+		mmio_clrsetbits_32(base +				\
+			TZC_REGION_OFFSET(				\
+				TZC_##macro_name##_REGION_SIZE,		\
+				region_no) +				\
+			TZC_##macro_name##_REGION_ATTR_0_OFFSET,	\
+			filters_mask << TZC_REGION_ATTR_F_EN_SHIFT,	\
+			filters << TZC_REGION_ATTR_F_EN_SHIFT);		\
+	}
+
+/*
+ * It is used to program region 0 ATTRIBUTES and ACCESS register.
+ */
+#define DEFINE_TZC_COMMON_CONFIGURE_REGION0(fn_name)			\
+	static void _tzc##fn_name##_configure_region0(uintptr_t base,	\
+			   unsigned int sec_attr,			\
+			   unsigned int ns_device_access)		\
+	{								\
+		assert(base != 0U);					\
+		VERBOSE("TrustZone : Configuring region 0 "		\
+			"(TZC Interface Base=0x%lx sec_attr=0x%x,"	\
+			" ns_devs=0x%x)\n", base,			\
+			sec_attr, ns_device_access);			\
+									\
+		/* Set secure attributes on region 0 */			\
+		_tzc##fn_name##_write_region_attributes(base, 0,	\
+			sec_attr << TZC_REGION_ATTR_SEC_SHIFT);		\
+									\
+		/***************************************************/	\
+		/* Specify which non-secure devices have permission*/	\
+		/* to access region 0.				   */	\
+		/***************************************************/	\
+		_tzc##fn_name##_write_region_id_access(base,		\
+						0,			\
+						ns_device_access);	\
+	}
+
+/*
+ * It is used to program a region from 1 to 8 in the TrustZone controller.
+ * NOTE:
+ * Region 0 is special; it is preferable to use
+ * ##fn_name##_configure_region0 for this region (see comment for
+ * that function).
+ */
+#define DEFINE_TZC_COMMON_CONFIGURE_REGION(fn_name)			\
+	static void _tzc##fn_name##_configure_region(uintptr_t base,	\
+				unsigned int filters,			\
+				unsigned int region_no,			\
+				unsigned long long region_base,		\
+				unsigned long long region_top,		\
+				unsigned int sec_attr,			\
+				unsigned int nsaid_permissions)		\
+	{								\
+		assert(base != 0U);					\
+		VERBOSE("TrustZone : Configuring region "		\
+			"(TZC Interface Base: 0x%lx, region_no = %u)"	\
+			"...\n", base, region_no);			\
+		VERBOSE("TrustZone : ... base = %llx, top = %llx,"	\
+			"\n", region_base, region_top);			\
+		VERBOSE("TrustZone : ... sec_attr = 0x%x,"		\
+			" ns_devs = 0x%x)\n",				\
+			sec_attr, nsaid_permissions);			\
+									\
+		/***************************************************/	\
+		/* Inputs look ok, start programming registers.    */	\
+		/* All the address registers are 32 bits wide and  */	\
+		/* have a LOW and HIGH				   */	\
+		/* component used to construct an address up to a  */	\
+		/* 64bit.					   */	\
+		/***************************************************/	\
+		_tzc##fn_name##_write_region_base(base,			\
+					region_no, region_base);	\
+		_tzc##fn_name##_write_region_top(base,			\
+					region_no, region_top);		\
+									\
+		/* Enable filter to the region and set secure attributes */\
+		_tzc##fn_name##_write_region_attributes(base,		\
+				region_no,				\
+				(sec_attr << TZC_REGION_ATTR_SEC_SHIFT) |\
+				(filters << TZC_REGION_ATTR_F_EN_SHIFT));\
+									\
+		/***************************************************/	\
+		/* Specify which non-secure devices have permission*/	\
+		/* to access this region.			   */	\
+		/***************************************************/	\
+		_tzc##fn_name##_write_region_id_access(base,		\
+						region_no,		\
+						nsaid_permissions);	\
+	}
+
+static inline unsigned int _tzc_read_peripheral_id(uintptr_t base)
+{
+	unsigned int id;
+
+	id = mmio_read_32(base + PID0_OFF);
+	/* Masks DESC part in PID1 */
+	id |= ((mmio_read_32(base + PID1_OFF) & 0xFU) << 8U);
+
+	return id;
+}
+
+#endif /* TZC_COMMON_PRIVATE_H */
diff --git a/drivers/arm/tzc/tzc_dmc500.c b/drivers/arm/tzc/tzc_dmc500.c
new file mode 100644
index 0000000..e45fbf8
--- /dev/null
+++ b/drivers/arm/tzc/tzc_dmc500.c
@@ -0,0 +1,287 @@
+/*
+ * Copyright (c) 2016-2018, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <drivers/arm/tzc_dmc500.h>
+#include <drivers/arm/tzc_common.h>
+#include <lib/mmio.h>
+
+#include "tzc_common_private.h"
+
+/*
+ * Macros which will be used by common core functions.
+ */
+#define TZC_DMC500_REGION_BASE_LOW_0_OFFSET		0x054
+#define TZC_DMC500_REGION_BASE_HIGH_0_OFFSET		0x058
+#define TZC_DMC500_REGION_TOP_LOW_0_OFFSET		0x05C
+#define TZC_DMC500_REGION_TOP_HIGH_0_OFFSET		0x060
+#define TZC_DMC500_REGION_ATTR_0_OFFSET			0x064
+#define TZC_DMC500_REGION_ID_ACCESS_0_OFFSET		0x068
+
+#define TZC_DMC500_ACTION_OFF				0x50
+
+/* Pointer to the tzc_dmc500_driver_data structure populated by the platform */
+static const tzc_dmc500_driver_data_t *g_driver_data;
+static unsigned int g_sys_if_count;
+
+#define verify_region_attr(region, attr)	\
+		((g_conf_regions[(region)].sec_attr ==			\
+			((attr) >> TZC_REGION_ATTR_SEC_SHIFT))		\
+		&& ((attr) & (0x1 << TZC_REGION_ATTR_F_EN_SHIFT)))
+
+/*
+ * Structure for configured regions attributes in DMC500.
+ */
+typedef struct tzc_dmc500_regions {
+	unsigned int sec_attr;
+	int is_enabled;
+} tzc_dmc500_regions_t;
+
+/*
+ * Array storing the attributes of the configured regions. This array
+ * will be used by the `tzc_dmc500_verify_complete` to verify the flush
+ * completion.
+ */
+static tzc_dmc500_regions_t g_conf_regions[MAX_REGION_VAL + 1];
+
+/* Helper Macros for making the code readable */
+#define DMC_INST_BASE_ADDR(instance) (g_driver_data->dmc_base[instance])
+#define DMC_INST_SI_BASE(instance, interface) \
+		(DMC_INST_BASE_ADDR(instance) + IFACE_OFFSET(interface))
+
+DEFINE_TZC_COMMON_WRITE_ACTION(_dmc500, DMC500)
+DEFINE_TZC_COMMON_WRITE_REGION_BASE(_dmc500, DMC500)
+DEFINE_TZC_COMMON_WRITE_REGION_TOP(_dmc500, DMC500)
+DEFINE_TZC_COMMON_WRITE_REGION_ATTRIBUTES(_dmc500, DMC500)
+DEFINE_TZC_COMMON_WRITE_REGION_ID_ACCESS(_dmc500, DMC500)
+
+DEFINE_TZC_COMMON_CONFIGURE_REGION0(_dmc500)
+DEFINE_TZC_COMMON_CONFIGURE_REGION(_dmc500)
+
+static inline unsigned int _tzc_dmc500_read_region_attr_0(
+					uintptr_t dmc_si_base,
+					unsigned int region_no)
+{
+	return mmio_read_32(dmc_si_base +
+			TZC_REGION_OFFSET(TZC_DMC500_REGION_SIZE, region_no) +
+			TZC_DMC500_REGION_ATTR_0_OFFSET);
+}
+
+static inline void _tzc_dmc500_write_flush_control(uintptr_t dmc_si_base)
+{
+	mmio_write_32(dmc_si_base + SI_FLUSH_CTRL_OFFSET, 1);
+}
+
+/*
+ * Sets the Flush controls for all the DMC Instances and System Interfaces.
+ * This initiates the flush of configuration settings from the shadow
+ * registers to the actual configuration register. The caller should poll
+ * changed register to confirm update.
+ */
+void tzc_dmc500_config_complete(void)
+{
+	int dmc_inst, sys_if;
+
+	assert(g_driver_data);
+
+	for (dmc_inst = 0; dmc_inst < g_driver_data->dmc_count; dmc_inst++) {
+		assert(DMC_INST_BASE_ADDR(dmc_inst));
+		for (sys_if = 0; sys_if < g_sys_if_count; sys_if++)
+			_tzc_dmc500_write_flush_control(
+					DMC_INST_SI_BASE(dmc_inst, sys_if));
+	}
+}
+
+/*
+ * This function reads back the secure attributes from the configuration
+ * register for each DMC Instance and System Interface and compares it with
+ * the configured value. The successful verification of the region attributes
+ * confirms that the flush operation has completed.
+ * If the verification fails, the caller is expected to invoke this API again
+ * till it succeeds.
+ * Returns 0 on success and 1 on failure.
+ */
+int tzc_dmc500_verify_complete(void)
+{
+	int dmc_inst, sys_if, region_no;
+	unsigned int attr;
+
+	assert(g_driver_data);
+	/* Region 0 must be configured */
+	assert(g_conf_regions[0].is_enabled);
+
+	/* Iterate over all configured regions */
+	for (region_no = 0; region_no <= MAX_REGION_VAL; region_no++) {
+		if (!g_conf_regions[region_no].is_enabled)
+			continue;
+		for (dmc_inst = 0; dmc_inst < g_driver_data->dmc_count;
+								dmc_inst++) {
+			assert(DMC_INST_BASE_ADDR(dmc_inst));
+			for (sys_if = 0; sys_if < g_sys_if_count;
+							sys_if++) {
+				attr = _tzc_dmc500_read_region_attr_0(
+					DMC_INST_SI_BASE(dmc_inst, sys_if),
+					region_no);
+				VERBOSE("Verifying DMC500 region:%d"
+					" dmc_inst:%d sys_if:%d attr:%x\n",
+					region_no, dmc_inst, sys_if, attr);
+				if (!verify_region_attr(region_no, attr))
+					return 1;
+			}
+		}
+	}
+
+	return 0;
+}
+
+/*
+ * `tzc_dmc500_configure_region0` is used to program region 0 in both the
+ * system interfaces of all the DMC-500 instances. Region 0 covers the whole
+ * address space that is not mapped to any other region for a system interface,
+ * and is always enabled; this cannot be changed. This function only changes
+ * the access permissions.
+ */
+void tzc_dmc500_configure_region0(unsigned int sec_attr,
+				  unsigned int nsaid_permissions)
+{
+	int dmc_inst, sys_if;
+
+	/* Assert if DMC-500 is not initialized */
+	assert(g_driver_data);
+
+	/* Configure region_0 in all DMC instances */
+	for (dmc_inst = 0; dmc_inst < g_driver_data->dmc_count; dmc_inst++) {
+		assert(DMC_INST_BASE_ADDR(dmc_inst));
+		for (sys_if = 0; sys_if < g_sys_if_count; sys_if++)
+			_tzc_dmc500_configure_region0(
+					DMC_INST_SI_BASE(dmc_inst, sys_if),
+					sec_attr, nsaid_permissions);
+	}
+
+	g_conf_regions[0].sec_attr = sec_attr;
+	g_conf_regions[0].is_enabled = 1;
+}
+
+/*
+ * `tzc_dmc500_configure_region` is used to program a region into all system
+ * interfaces of all the DMC instances.
+ * NOTE:
+ * Region 0 is special; it is preferable to use tzc_dmc500_configure_region0
+ * for this region (see comment for that function).
+ */
+void tzc_dmc500_configure_region(unsigned int region_no,
+			unsigned long long region_base,
+			unsigned long long region_top,
+			unsigned int sec_attr,
+			unsigned int nsaid_permissions)
+{
+	int dmc_inst, sys_if;
+
+	assert(g_driver_data);
+	/* Do range checks on regions. */
+	assert((region_no >= 0U) && (region_no <= MAX_REGION_VAL));
+
+	/*
+	 * Do address range check based on DMC-TZ configuration. A 43bit address
+	 * is the max and expected case.
+	 */
+	assert(((region_top <= (UINT64_MAX >> (64U - 43U))) &&
+		(region_base < region_top)));
+
+	/* region_base and (region_top + 1) must be 4KB aligned */
+	assert(((region_base | (region_top + 1U)) & (4096U - 1U)) == 0U);
+
+	for (dmc_inst = 0; dmc_inst < g_driver_data->dmc_count; dmc_inst++) {
+		assert(DMC_INST_BASE_ADDR(dmc_inst));
+		for (sys_if = 0; sys_if < g_sys_if_count; sys_if++)
+			_tzc_dmc500_configure_region(
+					DMC_INST_SI_BASE(dmc_inst, sys_if),
+					TZC_DMC500_REGION_ATTR_F_EN_MASK,
+					region_no, region_base, region_top,
+					sec_attr, nsaid_permissions);
+	}
+
+	g_conf_regions[region_no].sec_attr = sec_attr;
+	g_conf_regions[region_no].is_enabled = 1;
+}
+
+/* Sets the action value for all the DMC instances */
+void tzc_dmc500_set_action(unsigned int action)
+{
+	int dmc_inst;
+
+	assert(g_driver_data);
+
+	for (dmc_inst = 0; dmc_inst < g_driver_data->dmc_count; dmc_inst++) {
+		assert(DMC_INST_BASE_ADDR(dmc_inst));
+		/*
+		 * - Currently no handler is provided to trap an error via
+		 *   interrupt or exception.
+		 * - The interrupt action has not been tested.
+		 */
+		_tzc_dmc500_write_action(DMC_INST_BASE_ADDR(dmc_inst), action);
+	}
+}
+
+/*
+ * A DMC-500 instance must be present at each base address provided by the
+ * platform. It also expects platform to pass at least one instance of
+ * DMC-500.
+ */
+static void validate_plat_driver_data(
+			const tzc_dmc500_driver_data_t *plat_driver_data)
+{
+#if ENABLE_ASSERTIONS
+	int i;
+	unsigned int dmc_id;
+	uintptr_t dmc_base;
+
+	assert(plat_driver_data);
+	assert(plat_driver_data->dmc_count > 0 &&
+		(plat_driver_data->dmc_count <= MAX_DMC_COUNT));
+
+	for (i = 0; i < plat_driver_data->dmc_count; i++) {
+		dmc_base = plat_driver_data->dmc_base[i];
+		assert(dmc_base);
+
+		dmc_id = _tzc_read_peripheral_id(dmc_base);
+		assert(dmc_id == DMC500_PERIPHERAL_ID);
+	}
+#endif /* ENABLE_ASSERTIONS */
+}
+
+
+/*
+ * Initializes the base address and count of DMC instances.
+ *
+ * Note : Only pointer to plat_driver_data is saved, so it is caller's
+ * responsibility to keep it valid until the driver is used.
+ */
+void tzc_dmc500_driver_init(const tzc_dmc500_driver_data_t *plat_driver_data)
+{
+	/* Check valid pointer is passed */
+	assert(plat_driver_data);
+
+	/*
+	 * NOTE: This driver expects the DMC-500 controller is already in
+	 * READY state. Hence, it uses the reconfiguration method for
+	 * programming TrustZone regions
+	 */
+	/* Validates the information passed by platform */
+	validate_plat_driver_data(plat_driver_data);
+	g_driver_data = plat_driver_data;
+
+	/* Check valid system interface count */
+	assert(g_driver_data->sys_if_count <= MAX_SYS_IF_COUNT);
+
+	g_sys_if_count = g_driver_data->sys_if_count;
+
+	/* If interface count is not present then assume max */
+	if (g_sys_if_count == 0U)
+		g_sys_if_count = MAX_SYS_IF_COUNT;
+}
diff --git a/drivers/arm/tzc/tzc_dmc620.c b/drivers/arm/tzc/tzc_dmc620.c
new file mode 100644
index 0000000..7e307ee
--- /dev/null
+++ b/drivers/arm/tzc/tzc_dmc620.c
@@ -0,0 +1,177 @@
+/*
+ * Copyright (c) 2018-2020, ARM Limited and Contributors. All rights reserved.
+ *
+ * SPDX-License-Identifier: BSD-3-Clause
+ */
+
+#include <assert.h>
+
+#include <common/debug.h>
+#include <drivers/arm/tzc_dmc620.h>
+#include <lib/mmio.h>
+
+/* Mask to extract bit 31 to 16 */
+#define MASK_31_16 UINT64_C(0x0000ffff0000)
+/* Mask to extract bit 47 to 32 */
+#define MASK_47_32 UINT64_C(0xffff00000000)
+
+/* Helper macro for getting dmc_base addr of a dmc_inst */
+#define DMC_BASE(plat_data, dmc_inst) \
+	((uintptr_t)((plat_data)->dmc_base[(dmc_inst)]))
+
+/* Pointer to the tzc_dmc620_config_data structure populated by the platform */
+static const tzc_dmc620_config_data_t *g_plat_config_data;
+
+#if ENABLE_ASSERTIONS
+/*
+ * Helper function to check if the DMC-620 instance is present at the
+ * base address provided by the platform and also check if at least
+ * one dmc instance is present.
+ */
+static void tzc_dmc620_validate_plat_driver_data(
+			const tzc_dmc620_driver_data_t *plat_driver_data)
+{
+	unsigned int dmc_inst, dmc_count, dmc_id;
+	uintptr_t base;
+
+	assert(plat_driver_data != NULL);
+
+	dmc_count = plat_driver_data->dmc_count;
+	assert(dmc_count > 0U);
+
+	for (dmc_inst = 0U; dmc_inst < dmc_count; dmc_inst++) {
+		base = DMC_BASE(plat_driver_data, dmc_inst);
+		dmc_id = mmio_read_32(base + DMC620_PERIPHERAL_ID_0);
+		assert(dmc_id == DMC620_PERIPHERAL_ID_0_VALUE);
+	}
+}
+#endif
+
+/*
+ * Program a region with region base and region top addresses of all
+ * DMC-620 instances.
+ */
+static void tzc_dmc620_configure_region(int region_no,
+					unsigned long long region_base,
+					unsigned long long region_top,
+					unsigned int sec_attr)
+{
+	uint32_t min_31_00, min_47_32;
+	uint32_t max_31_00, max_47_32;
+	unsigned int dmc_inst, dmc_count;
+	uintptr_t base;
+	const tzc_dmc620_driver_data_t *plat_driver_data;
+
+	plat_driver_data = g_plat_config_data->plat_drv_data;
+	assert(plat_driver_data != NULL);
+
+	/* Do range checks on regions. */
+	assert((region_no >= 0) && (region_no <= DMC620_ACC_ADDR_COUNT));
+
+	/* region_base and (region_top + 1) must be 4KB aligned */
+	assert(((region_base | (region_top + 1U)) & (4096U - 1U)) == 0U);
+
+	dmc_count = plat_driver_data->dmc_count;
+	for (dmc_inst = 0U; dmc_inst < dmc_count; dmc_inst++) {
+		min_31_00 = (uint32_t)((region_base & MASK_31_16) | sec_attr);
+		min_47_32 = (uint32_t)((region_base & MASK_47_32)
+				>> DMC620_ACC_ADDR_WIDTH);
+		max_31_00 = (uint32_t)(region_top  & MASK_31_16);
+		max_47_32 = (uint32_t)((region_top  & MASK_47_32)
+				>> DMC620_ACC_ADDR_WIDTH);
+
+		/* Extract the base address of the DMC-620 instance */
+		base = DMC_BASE(plat_driver_data, dmc_inst);
+		/* Configure access address region registers */
+		mmio_write_32(base + DMC620_ACC_ADDR_MIN_31_00_NEXT(region_no),
+				min_31_00);
+		mmio_write_32(base + DMC620_ACC_ADDR_MIN_47_32_NEXT(region_no),
+				min_47_32);
+		mmio_write_32(base + DMC620_ACC_ADDR_MAX_31_00_NEXT(region_no),
+				max_31_00);
+		mmio_write_32(base + DMC620_ACC_ADDR_MAX_47_32_NEXT(region_no),
+				max_47_32);
+	}
+}
+
+/*
+ * Set the action value for all the DMC-620 instances.
+ */
+static void tzc_dmc620_set_action(void)
+{
+	unsigned int dmc_inst, dmc_count;
+	uintptr_t base;
+	const tzc_dmc620_driver_data_t *plat_driver_data;
+
+	plat_driver_data = g_plat_config_data->plat_drv_data;
+	dmc_count = plat_driver_data->dmc_count;
+	for (dmc_inst = 0U; dmc_inst < dmc_count; dmc_inst++) {
+		/* Extract the base address of the DMC-620 instance */
+		base = DMC_BASE(plat_driver_data, dmc_inst);
+		/* Switch to READY */
+		mmio_write_32(base + DMC620_MEMC_CMD, DMC620_MEMC_CMD_GO);
+		mmio_write_32(base + DMC620_MEMC_CMD, DMC620_MEMC_CMD_EXECUTE);
+	}
+}
+
+/*
+ * Verify whether the DMC-620 configuration is complete by reading back
+ * configuration registers and comparing it with the configured value. If
+ * configuration is incomplete, loop till the configured value is reflected in
+ * the register.
+ */
+static void tzc_dmc620_verify_complete(void)
+{
+	unsigned int dmc_inst, dmc_count;
+	uintptr_t base;
+	const tzc_dmc620_driver_data_t *plat_driver_data;
+
+	plat_driver_data = g_plat_config_data->plat_drv_data;
+	dmc_count = plat_driver_data->dmc_count;
+	for (dmc_inst = 0U; dmc_inst < dmc_count; dmc_inst++) {
+		/* Extract the base address of the DMC-620 instance */
+		base = DMC_BASE(plat_driver_data, dmc_inst);
+		while ((mmio_read_32(base + DMC620_MEMC_STATUS) &
+				DMC620_MEMC_CMD_MASK) != DMC620_MEMC_CMD_GO) {
+			continue;
+		}
+	}
+}
+
+/*
+ * Initialize the DMC-620 TrustZone Controller using the region configuration
+ * supplied by the platform. The DMC620 controller should be enabled elsewhere
+ * before invoking this function.
+ */
+void arm_tzc_dmc620_setup(const tzc_dmc620_config_data_t *plat_config_data)
+{
+	uint8_t i;
+
+	/* Check if valid pointer is passed */
+	assert(plat_config_data != NULL);
+
+	/*
+	 * Check if access address count passed by the platform is less than or
+	 * equal to DMC620's access address count
+	 */
+	assert(plat_config_data->acc_addr_count <= DMC620_ACC_ADDR_COUNT);
+
+#if ENABLE_ASSERTIONS
+	/* Validates the information passed by platform */
+	tzc_dmc620_validate_plat_driver_data(plat_config_data->plat_drv_data);
+#endif
+
+	g_plat_config_data = plat_config_data;
+
+	INFO("Configuring DMC-620 TZC settings\n");
+	for (i = 0U; i < g_plat_config_data->acc_addr_count; i++) {
+		tzc_dmc620_configure_region(i,
+			g_plat_config_data->plat_acc_addr_data[i].region_base,
+			g_plat_config_data->plat_acc_addr_data[i].region_top,
+			g_plat_config_data->plat_acc_addr_data[i].sec_attr);
+	}
+
+	tzc_dmc620_set_action();
+	tzc_dmc620_verify_complete();
+	INFO("DMC-620 TZC setup completed\n");
+}